/***************************************************************************
 创建者: 华磊
 开始时间: 2017.12.15
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file coordinatetrajectory.h
 *  功能:
 * 轴对象的管理和对外接口。
 *
 *                                                                         *
 ***************************************************************************/
#include "coordinatetrajectory.h"
#include "qdebug.h"
#include <frames.hpp>
#include <frames_io.hpp>
#include <trajectory_composite.hpp>
#include <trajectory.hpp>
#include <trajectory_segment.hpp>
#include <trajectory_stationary.hpp>
#include <trajectory_composite.hpp>
#include <velocityprofile_trap.hpp>
#include <path_roundedcomposite.hpp>
#include <rotational_interpolation_sa.hpp>
#include <velocityprofile_spline.hpp>
#include <velocityprofile_rect.hpp>
#include <utilities/error.h>
#include <path_circle.hpp>
#include <Eigen/Dense>
#include "path_3bezier.h"
#include "velocityprofile_improvedspline.h"
#include "velocityprofile_sin.h"

//#define D_ANGLE_NOT_ROTATE M_PI*3.0/180.0
#define D_ANGLE_NOT_ROTATE 0.052
#define D_ROTATE_SPLINE_ESTIMATE_RATIO 7.0
#define D_ZERO_VELOCITY 0.00001
using namespace Eigen;
CoordinateTrajectory::CoordinateTrajectory(int robotIdIn, double deltaTimeIn, JointAttribute axisAttributeIn,
                   MotionMessage *motionMessageIn, double rotate_eqradiusIn, double allowVelFeedbackBiasRatioIn,
                                           double rotate_maxVelIn, double rotate_maxAccIn)
{

    robotId=robotIdIn;
    motionMessage=motionMessageIn;
//    trajComposite=NULL;
    timeAxisScale=1.0;
    timeAxisLimitRatio=1.0;
    deltaTime=deltaTimeIn;
    axisAbstractType=E_AXIS_ABSTRACT_VIRTUAL;
    axisIdByRobot=0;
    loadAxisAttribute(axisAttributeIn,rotate_eqradiusIn,rotate_maxVelIn,rotate_maxAccIn);
    currentRunningLine=-1;
    iscurrentAxisCommandFInished=-1;
    currentCommandRemainingTime=-1;
    totalRemainingTime=-1;
    linesNumberQueue.clear();
    linesDurationQueue.clear();
    noRotateMoveAngle=0.050;
    lastCommand_velocity_line=0;

    velCommand.Zero();
    accCommand.Zero();
    torqueCommand.Zero();

    velFeedback.Zero();
    torqueFeedback.Zero();

    velFollowOffset.Zero();
    torqueFollowOffset.Zero();
    axisAccelerationRatio=1.0;
//    accumulatedTime=0.0;
    basicStatus=E_AXIS_STATUS_NORMAL;
    limitCheckMode=E_CHECK_MODE_T2;
    isPositionCommandExceed=0;
    isVelCommandExceed=0;
    isAccCommandExceed=0;
    isTorqueCommandExceed=0;
    isProcessFollowErrorExceed=0;
    currentPathType=E_NAV_COMMAND_NONE;

    allowVelFeedbackBiasRatio=allowVelFeedbackBiasRatioIn;

//    for(int i=0;i<VEL_PROFILE_ARRY_MAX;i++)
//    {
//        trajCompositeArry[i]=NULL;
//    }
//    currentTrajCompositeArryIndex=-1;

    resetAll();

}

int CoordinateTrajectory::setRobotCoordinateConfig(RobotCoordinateAxisConfig coordinateConfigIn)
{
    rotate_eqradius=coordinateConfigIn.rotate_eqradius/1000.0;
    t1RatioLimit=coordinateConfigIn.t1RatioLimit;
    maxAccelation_line=coordinateConfigIn.lineMaxAcceleration/1000.0;//mm/s转换为m/s
    maxVelocity_line=coordinateConfigIn.lineMaxSpeed/1000.0;//mm/s转换为m/s
    return 1;

}

bool CoordinateTrajectory::getMixRatioOut(double &mixedRatioOut)
{
    mixedRatioOut=positionMixRatio;
    return isInPositionMixsing;
}

int CoordinateTrajectory::setRotateEqradius(double rotate_eqradiusIn)
{
    qDebug()<<"setRotateEqradius: old"<<rotate_eqradius*1000<<" new"<<rotate_eqradiusIn;
    rotate_eqradius=rotate_eqradiusIn/1000.0;
    return 1;
}

int CoordinateTrajectory::getVehicleTrajectoryControlParameter(VehicleTrajectoryControlParameter &infoOut)
{
    infoOut.maxVel_line=maxVelocity_line;
    infoOut.maxAcc_line=maxAccelation_line;
    infoOut.maxVel_rotate=maxVelocity_rotate;
    infoOut.maxAcc_rotate=maxAccelation_rotate;

    return 1;
}

int CoordinateTrajectory::setVehicleTrajectoryControlParameter(VehicleTrajectoryControlParameter infoIn, int &returnFlag)
{
    maxVelocity_line=infoIn.maxVel_line;
    maxAccelation_line=infoIn.maxAcc_line;
    maxVelocity_rotate=infoIn.maxVel_rotate;
    maxAccelation_rotate=infoIn.maxAcc_rotate;
    returnFlag=1;
    return 1;
}

int CoordinateTrajectory::setVelocityStyle(E_VELOCITY_TYPE velocityStypeIn)
{
    velocityStype=velocityStypeIn;
    return 1;
}

double CoordinateTrajectory::getTrajectoryPoint_steeringAngle()
{
    return 180.0*vehiclePositionCommand.rotate/M_PI;
}

int CoordinateTrajectory::setSteeringMotionParameter(double maxVelIn, double maxAccIn,bool isSteeringValidIn)
{
    steeringMaxAcceration=maxAccIn;
    steeringMaxSpeed=maxVelIn;
    isSteeringValid=isSteeringValidIn;
    return 1;
}

bool CoordinateTrajectory::isLastLineVelocityZero()
{
    if(lastCommand_velocity_line<D_ZERO_VELOCITY)
    {
        return true;
    }
    return false;

}

int CoordinateTrajectory::setNoRatateMoveAngle(double angleIn)
{
    noRotateMoveAngle=angleIn;
    return 1;
}

int CoordinateTrajectory::setAxisFeedbackVelocity(double transVelIn, double rotateVelIn)
{
    velFeedback.vel[0]=transVelIn;
    return 1;
}

int CoordinateTrajectory::setMaxVelocity(double maxVelIn)
{
    maxVelocity_line=maxVelIn/1000.0;
    return 1;
}

int CoordinateTrajectory::setAccelerationRatio(double ratioIn)
{
        qDebug()<<"CoordinateTrajectory::setAccelerationRatio ratioIn"<<ratioIn;
        if(ratioIn>0)
        {
            accelerationRatio=ratioIn;
        }

        return 1;
}

int CoordinateTrajectory::setAcceleration(double accelerationIn)
{
//    qDebug()<<"setAcceleration before　maxAccelation_line"<<maxAccelation_line;
    maxAccelation_line=accelerationIn/1000.0;
//    qDebug()<<"setAcceleration after　maxAccelation_line"<<maxAccelation_line
//           <<"maxVelocity_line"<<maxVelocity_line;
    return 1;
}

int CoordinateTrajectory::setAcceleration_rotate(double accelerationIn)
{
//    qDebug()<<"setAcceleration_rotate before，maxAccelation_rotate"<<maxAccelation_rotate;
    maxAccelation_rotate=accelerationIn/1000.0;
//    qDebug()<<"setAcceleration_rotate after，maxAccelation_rotate"<<maxAccelation_rotate
//              <<"maxVelocity_rotate"<<maxVelocity_rotate;
    return 1;
}

int CoordinateTrajectory::getSegmentMotionId(E_MOTION_ID &idIn)
{
    double accumulatedTime=getAccumulatedTime();
    if(0==segmentAtrributeQueue.size())
    {
        idIn=E_MOTION_ID_NONE;
        return 1;
    }
    for(int i=0;i<segmentAtrributeQueue.size();i++)
    {
        if(0==i)
        {
            if(accumulatedTime<segmentAtrributeQueue[i].timeStamp
                    && accumulatedTime>deltaTime)
            {
                //可能出现误判为直线类型。轨迹最后结束时。
                //可能该段结束后，下一代的点还没有计算出来，如果直接用下一个点，可能出现逻辑错误。
                idIn=segmentAtrributeQueue[i].motionId;
                return 1;
            }
        }
        else
        {
            if(accumulatedTime<segmentAtrributeQueue[i].timeStamp
                    && accumulatedTime>segmentAtrributeQueue[i-1].timeStamp+deltaTime)
            {
                //可能出现误判为直线类型。轨迹最后结束时。
                //可能该段结束后，下一代的点还没有计算出来，如果直接用下一个点，可能出现逻辑错误。
                idIn=segmentAtrributeQueue[i].motionId;
                return 1;
            }
        }


    }
    idIn=E_MOTION_ID_NONE;
    return -1;
}

//double CoordinateTrajectory::getTrajectoryRemainningTime()
//{
//    return totalRemainingTime;

//}
bool CoordinateTrajectory::isInFineMoving()
{
//    if(isTrajComposeEmpty() || -1==currentTrajCompositeArryIndex || currentCommandRemainingTime<deltaTime)
//    if(isTrajComposeEmpty()  || currentCommandRemainingTime<deltaTime  )
    if(1>=trajAttributeQueue.size() )
    {
        return true;
    }
    else
    {
        return false;
    }
    return false;
}

int CoordinateTrajectory::isVelProfileEmpty()
{
    if(isTrajComposeEmpty() )
    {
        return 1;
    }
    else
    {
        return 0;
    }
    return 0;
}


//int CoordinateTrajectory::resetTraj()
//{
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
//    deleteTrajComposite();
//    return 1;
//}

int CoordinateTrajectory::resetAll()
{
    isInPositionMixsing=false;
    clearTraj();
//    deleteTrajComposite();
//    currentTrajCompositeArryIndex=-1;



    positionCommand=positionFeedback;//todo positionFeedback not set.
    positionFrameCommand=positionFrameFeedback;
    positionFrameCommand_planed=positionFrameCommand;
    vehiclePositionCommand=vehiclePositionFeedback;
    if(0)
    {
        qDebug()<<"CoordinateTrajectory::resetAll"<<positionFrameCommand.p[0]<<positionFrameCommand.p[1]<<positionFrameCommand.p[2];
    }

    positionCommandOld=positionCommand;
    positionFrameCommandOld=positionFrameCommand;
    vehiclePositionCommandOld=vehiclePositionCommand;

    targetPosition=vehiclePositionCommand;
    currentPathType=E_NAV_COMMAND_NONE;

    velCommand=velFeedback;
    torqueCommand=torqueFeedback;
    accCommand.Zero();
//    accumulatedTime=0.0;
    isPositionCommandExceed=0;
    isVelCommandExceed=0;
    isAccCommandExceed=0;
    isTorqueCommandExceed=0;
    isProcessFollowErrorExceed=0;
    basicStatus=E_AXIS_STATUS_NORMAL;
    lastCommand_velocity_line=0;

    tmp_exceedCount=0;
    tmp_velFeedback_old=0;
    accelerationRatio=1;

    return 1;
}


int CoordinateTrajectory::calculateFollowOffset()
{
//    positionFollowOffset = positionCommand-positionFeedback;
//    velFollowOffset = velCommand-velFeedback;
//    torqueFollowOffset =  torqueCommand-torqueFeedback;
    return 1;
}
int CoordinateTrajectory::returnAxisCommandData(PointPro &returnedPosition, Twist &returnedVel, Twist &returnedAcc, Twist &returnedTorque )
{
    returnedPosition=positionCommand;
    returnedVel=velCommand;
    returnedAcc=accCommand;
    returnedTorque=torqueCommand;
    return 1;
}

int CoordinateTrajectory::returnAxisFeedbackData(PointPro &returnedPosition, Twist &returnedVel, Twist &returnedTorque )
{
    returnedPosition=positionFeedback;
    returnedVel=velFeedback;
    returnedTorque=torqueFeedback;
    return 1;
}

int CoordinateTrajectory::returnAxisFollowOffsetData(PointPro &returnedPosition, Twist &returnedVel, Twist &returnedTorque )
{
    returnedPosition=positionOffset;
    returnedVel=velFollowOffset;
    returnedTorque=torqueFollowOffset;
    return 1;
}

int CoordinateTrajectory::loadAxisAttribute(JointAttribute axisAttributeIn, double rotate_eqradiusIn,
                                            double rotate_maxVelIn, double rotate_maxAccIn)
{

        rotate_eqradius=rotate_eqradiusIn/1000.0;

        t1RatioLimit=axisAttributeIn.t1RatioLimit;
        coulombFriction=axisAttributeIn.coulombFriction;
        coupleFlag=axisAttributeIn.coupleFlag;
        coupleJoint=axisAttributeIn.coupleJoint;
        coupleReducer=axisAttributeIn.coupleReducer;
        for(int i=0;i<4;i++)
        {
            dhParameter[i]=axisAttributeIn.dhParameter[i];
        }
        dhType=axisAttributeIn.dhType;
        encoderResolution=axisAttributeIn.encoderResolution;
        pulsePerRound=axisAttributeIn.pulsePerRound;
        externAxisFlag=axisAttributeIn.externAxisFlag;
        gearRatio=axisAttributeIn.gearRatio;
        jointName=axisAttributeIn.jointName;
        mass=axisAttributeIn.mass;
        massCenter=axisAttributeIn.massCenter;
        inertiaMatrix=axisAttributeIn.inertiaMatrix;
        maxAccelation_line=axisAttributeIn.maxAccelation/1000.0;//mm/s转换为m/s
        maxAccelation_rotate=rotate_maxAccIn/1000.0;//通过旋转半径，将线速度等效为角速度。
//        maxPosition=axisAttributeIn.maxPosition;
        maxTorque=axisAttributeIn.maxTorque;
        jogTorqueLimit=axisAttributeIn.jogTorqueLimit;
        maxVelocity_line=axisAttributeIn.maxVelocity/1000.0;//mm/s转换为m/s
        maxVelocity_rotate=rotate_maxVelIn/1000.0;//通过旋转半径，将线速度等效为角速度。
//        minPosition=axisAttributeIn.minPosition;
        mobile=axisAttributeIn.mobile;
//        positionOffset=axisAttributeIn.positionOffset;
        processFollowError=axisAttributeIn.processFollowError;
        screwPitch=axisAttributeIn.screwPitch;
        targetFollowError=axisAttributeIn.targetFollowError;
        viscousFriction=axisAttributeIn.viscousFriction;

        return 1;
}
int CoordinateTrajectory::setTimeAxisScale(double ratio)
{
    if(0>ratio || 1<ratio)
    {
        return -1;
    }
    else
    {
        timeAxisScale=ratio*timeAxisLimitRatio;
    }
//    qDebug()<<"setTimeAxisScale"<<timeAxisScale<<"timeAxisLimitRatio"<<timeAxisLimitRatio;
    return 1;
}

int CoordinateTrajectory::setNewTimeAxisVelLimitRatio(double scaleIn)
{
    timeAxisLimitRatio*=scaleIn;
    return 1;
}

int CoordinateTrajectory::restoreTimeAxisVelLimitRatio(double scaleIn)
{
    if(1>timeAxisLimitRatio)
    {
        timeAxisLimitRatio+=deltaTime;
//        timeAxisLimitRatio*=scaleIn;
        if(1<timeAxisLimitRatio)
        {
            timeAxisLimitRatio=1;
        }
    }
    return 1;
}

int CoordinateTrajectory::resetTimeAxisVelLimitRatio()
{
    timeAxisLimitRatio=1;
    return 1;
}

int CoordinateTrajectory::setDeltaTime(double time)
{
    deltaTime=time;
    return 1;
}

int CoordinateTrajectory::moveOneStep()
{

    if(isTrajComposeEmpty())
    {
//        qDebug()<<"CoordinateTrajectory::moveOneStep() isTrajComposeEmpty()";
        return -1;
    }

    double accumulatedTime=getAccumulatedTime();
    double tmpDuration=getTrajComposite()->Duration();
    if(tmpDuration<deltaTime)//double 如果溢出.
    {
//        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6009,robotId);
        qDebug()<<"warnning,,CoordinateTrajectory tmpDuration<deltaTime";
        clearTraj();
        return -1;
    }

    if(accumulatedTime <=tmpDuration )
    {
//        if(accumulatedTime <= tmpDuration  && accumulatedTime >tmpDuration -3*deltaTime)
//        {
//             qDebug()<<"vel end point"<<velCommand <<  "axis ID:"<< axisIdByRobot;
//        }

        accCommand= timeAxisScale*timeAxisScale*getTrajComposite()->Acc(accumulatedTime);
        velCommand=timeAxisScale*getTrajComposite()->Vel(accumulatedTime);
        positionFrameCommandOld=positionFrameCommand;
        vehiclePositionCommandOld=vehiclePositionCommand;
        positionFrameCommand= getTrajComposite()->Pos(accumulatedTime);
        vehiclePositionCommand=transferFrameToVehiclePosition(positionFrameCommand);
//        PointPro tmpPoint=coordinateSolver->frameToPointPro_CART(pointFrame);
        currentCommandRemainingTime=tmpDuration-accumulatedTime;
        //qDebug()<<"vel----"<<velCommand;
    }
    else
    {
        return -1;
    }
    //更新time
    accumulatedTime=accumulatedTimeMoveOneStep();
    //为了精确到达，后面可能要取消
//    if(accumulatedTime > trajComposite->Duration() &&  accumulatedTime < trajComposite->Duration()+deltaTime )
//    {
//        accumulatedTime=trajComposite->Duration();
//    }
    //这个逻辑有问题，虽然可以精确到达，但是行号判断时差ｄeltaTime,就认为完成了。timeAxisScale=0.001时可能要走很多个周期。
    if(accumulatedTime > tmpDuration)
    {
        clearTraj();

    }


    if(-1==isLimitCheckOk())
    {
        return -2;
    }
    else
    {
        return 1;
    }

    return 1;
}

int CoordinateTrajectory::moveOneStepByTrajIndex(int trajIndex)
{

    if(isTrajComposeEmpty())
    {
        positionFrameCommand_cnt=positionFrameCommand;
        return -1;
    }
    double tmpDuration;
    double accumulatedTime=getAccumulatedTime(trajIndex);
    Trajectory_Composite * tmpTraj;
    tmpTraj=getTrajComposite(trajIndex);
    tmpDuration=tmpTraj->Duration();


    if(tmpDuration<deltaTime)//double 如果溢出.
    {
//        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6009,robotId);
        qDebug()<<"warnning,,CoordinateTrajectory2 tmpDuration<deltaTime";
//        clearTraj();
        setAccumulatedTime(tmpDuration,trajIndex);
        return 0;
    }

    if(accumulatedTime <=tmpDuration )
    {
//        if(accumulatedTime <= tmpDuration  && accumulatedTime >tmpDuration -3*deltaTime)
//        {
//             qDebug()<<"vel end point"<<velCommand <<  "axis ID:"<< axisIdByRobot;
//        }

        accCommand_mixed= timeAxisScale*timeAxisScale*tmpTraj->Acc(accumulatedTime);
        velCommand_mixed=timeAxisScale*tmpTraj->Vel(accumulatedTime);
//        positionFrameCommandOld=positionFrameCommand;
//        vehiclePositionCommandOld=vehiclePositionCommand;
        positionFrameCommand_cnt= tmpTraj->Pos(accumulatedTime);
//        vehiclePositionCommand_mixed=transferFrameToVehiclePosition(positionFrameCommand_mixed);
//        PointPro tmpPoint=coordinateSolver->frameToPointPro_CART(pointFrame);
//        currentCommandRemainingTime_secondTraj=tmpDuration-accumulatedTime_secondTraj;
        //qDebug()<<"vel----"<<velCommand;
//        qDebug()<<"accumulatedTime_secondTraj accumulatedTime_secondTraj"<<accumulatedTime_secondTraj;
        if(0)
        {
    //        clearTraj();
            qDebug()<<"coor-moveOneStepByTrajIndex index"<<trajIndex<<"accumulatedTime"<<  accumulatedTime
                   <<"tmpDuration"<<tmpDuration<<"pos"<<positionFrameCommand_cnt.p[0]
                  <<positionFrameCommand_cnt.p[1]<<positionFrameCommand_cnt.p[2];
        }
    }
    else
    {
        return 0;
    }
    //更新time
    accumulatedTimeMoveOneStep(trajIndex);
    //为了精确到达，后面可能要取消
//    if(accumulatedTime > trajComposite->Duration() &&  accumulatedTime < trajComposite->Duration()+deltaTime )
//    {
//        accumulatedTime=trajComposite->Duration();
//    }
    //这个逻辑有问题，虽然可以精确到达，但是行号判断时差ｄeltaTime,就认为完成了。timeAxisScale=0.001时可能要走很多个周期。
    if(accumulatedTime > tmpDuration)
    {
//        clearTraj();
        qDebug()<<"moveOneStepByTrajIndex accumulatedTime > tmpDuration"<<accumulatedTime<<tmpDuration;
        return 0;

    }




    return isLimitCheckOk();
}

int CoordinateTrajectory::moveOneStep_secondTraj()
{

//    if(2>trajAttributeQueue.size())
//    {
////        qDebug()<<"CoordinateTrajectory::moveOneStep() isTrajComposeEmpty()";
//        return -1;
//    }

//    double tmpDuration=getTrajComposite_secondTraj()->Duration();
//    if(tmpDuration<deltaTime)//double 如果溢出.
//    {
////        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6009,robotId);
//        qDebug()<<"warnning,,CoordinateTrajectory2 tmpDuration<deltaTime";
////        clearTraj();
//        return -1;
//    }

//    if(accumulatedTime_secondTraj <=tmpDuration )
//    {
////        if(accumulatedTime <= tmpDuration  && accumulatedTime >tmpDuration -3*deltaTime)
////        {
////             qDebug()<<"vel end point"<<velCommand <<  "axis ID:"<< axisIdByRobot;
////        }

//        accCommand_mixed= timeAxisScale*timeAxisScale*getTrajComposite_secondTraj()->Acc(accumulatedTime_secondTraj);
//        velCommand_mixed=timeAxisScale*getTrajComposite_secondTraj()->Vel(accumulatedTime_secondTraj);
////        positionFrameCommandOld=positionFrameCommand;
////        vehiclePositionCommandOld=vehiclePositionCommand;
//        positionFrameCommand_mixed= getTrajComposite_secondTraj()->Pos(accumulatedTime_secondTraj);
//        vehiclePositionCommand_mixed=transferFrameToVehiclePosition(positionFrameCommand_mixed);
////        PointPro tmpPoint=coordinateSolver->frameToPointPro_CART(pointFrame);
//        currentCommandRemainingTime_secondTraj=tmpDuration-accumulatedTime_secondTraj;
//        //qDebug()<<"vel----"<<velCommand;
////        qDebug()<<"accumulatedTime_secondTraj accumulatedTime_secondTraj"<<accumulatedTime_secondTraj;
//    }
//    else
//    {
//        return -1;
//    }
//    //更新time
//    accumulatedTime_secondTraj+=timeAxisScale*deltaTime;
//    //为了精确到达，后面可能要取消
////    if(accumulatedTime > trajComposite->Duration() &&  accumulatedTime < trajComposite->Duration()+deltaTime )
////    {
////        accumulatedTime=trajComposite->Duration();
////    }
//    //这个逻辑有问题，虽然可以精确到达，但是行号判断时差ｄeltaTime,就认为完成了。timeAxisScale=0.001时可能要走很多个周期。
//    if(accumulatedTime_secondTraj > tmpDuration)
//    {
////        clearTraj();
//        qDebug()<<"accumulatedTime_secondTraj > tmpDuration"<<accumulatedTime_secondTraj<<tmpDuration;
//        return -3;

//    }


//    if(-1==isLimitCheckOk())
//    {
//        return -2;
//    }
//    else
//    {
//        return 1;
//    }

//    return 1;
}

int CoordinateTrajectory::moveOneStep_mixedPosition()
{
//    if(isTrajComposeEmpty())
//    {
////        qDebug()<<"CoordinateTrajectory::moveOneStep_mixedPosition() isTrajComposeEmpty()";
//        return -1;
//    }

//    double tmpDuration=getTrajComposite()->Duration();
//    if(tmpDuration<deltaTime)//double 如果溢出.
//    {
////        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6009,robotId);
//        qDebug()<<"warnning,,CoordinateTrajectory tmpDuration<deltaTime";
//        clearTraj();
//        return -1;
//    }

//    if(accumulatedTime <=tmpDuration )
//    {
////        if(accumulatedTime <= tmpDuration  && accumulatedTime >tmpDuration -3*deltaTime)
////        {
////             qDebug()<<"vel end point"<<velCommand <<  "axis ID:"<< axisIdByRobot;
////        }

//        accCommand= timeAxisScale*timeAxisScale*getTrajComposite()->Acc(accumulatedTime);
//        velCommand=timeAxisScale*getTrajComposite()->Vel(accumulatedTime);
//        positionFrameCommandOld=positionFrameCommand;
//        vehiclePositionCommandOld=vehiclePositionCommand;
//        positionFrameCommand= getTrajComposite()->Pos(accumulatedTime);
//        vehiclePositionCommand=transferFrameToVehiclePosition(positionFrameCommand);
////        PointPro tmpPoint=coordinateSolver->frameToPointPro_CART(pointFrame);
//        currentCommandRemainingTime=tmpDuration-accumulatedTime;
//        //qDebug()<<"vel----"<<velCommand;

//        if(trajAttributeQueue.size()>1)
//        {
//            TrajeAttribute tmpTrajAttri=trajAttributeQueue.front();
//            if(currentCommandRemainingTime<=tmpTrajAttri.endCutoffTime &&
//                                                tmpTrajAttri.cnt>0)
//            {
////                qDebug()<<"mixed----accumulatedTime"<<accumulatedTime;

//                if(1==moveOneStep_secondTraj())
//                {
//                    positionMixRatio=1.0-currentCommandRemainingTime/tmpTrajAttri.endCutoffTime;
//                    isInPositionMixsing=true;
//                }
//                else
//                {
//                    isInPositionMixsing=false;
//                }

//            }

//        }

//    }
//    else
//    {
//        isInPositionMixsing=false;
//        return -1;
//    }
//    //更新time
//    accumulatedTime+=timeAxisScale*deltaTime;
//    //为了精确到达，后面可能要取消
////    if(accumulatedTime > trajComposite->Duration() &&  accumulatedTime < trajComposite->Duration()+deltaTime )
////    {
////        accumulatedTime=trajComposite->Duration();
////    }
//    //这个逻辑有问题，虽然可以精确到达，但是行号判断时差ｄeltaTime,就认为完成了。timeAxisScale=0.001时可能要走很多个周期。
//    if(accumulatedTime > tmpDuration)
//    {
//        qDebug()<<"clearTraj_front----accumulatedTime"<<accumulatedTime;
//        isInPositionMixsing=false;
//        clearTraj_front();

//    }


//    if(-1==isLimitCheckOk())
//    {
//        return -2;
//    }
//    else
//    {
//        return 1;
//    }

//    return 1;
}

int CoordinateTrajectory::clearTraj()
{

    deleteTrajComposite();

    return 1;
}

int CoordinateTrajectory::clearTraj_front()
{
    if(trajAttributeQueue.size()>0)
    {
//        if(trajAttributeQueue.front().cnt>0)
//        {
//            qDebug()<<"accumulatedTime=accumulatedTime_secondTraj"<<accumulatedTime_secondTraj;
////            accumulatedTime=accumulatedTime_secondTraj;
////            accumulatedTime_secondTraj=0;
////            positionFrameCommand=positionFrameCommand_mixed;
////            velCommand=velCommand_mixed;
////            accCommand=accCommand_mixed;
//        }
//        else
//        {
//            accumulatedTime=0;
//            accumulatedTime_secondTraj=0;

//        }
        delete trajAttributeQueue.front().trajComp;
        trajAttributeQueue.dequeue();
        return 1;
    }
    return 0;
}

//int CoordinateTrajectory::revertTime(double timeDuration)
//{
//    if(accumulatedTime-timeDuration<=0)
//    {
//        return -1;
//    }
//    accumulatedTime=accumulatedTime-timeDuration;
//    return 1;
//}

int CoordinateTrajectory::moveOneStepByPosition(PointPro positionIn)
{
    if(NULL!=getTrajComposite())
    {
        return -1;
    }
        positionCommand=positionIn;

    return isLimitCheckOk();
}

int CoordinateTrajectory::moveOneStepByPV(PointPro positionIn, Twist velIn )
{
    if(NULL!=getTrajComposite())
    {
        return -1;

    }

    positionCommand=positionIn;
    velCommand=velIn;

    return isLimitCheckOk();
}

int CoordinateTrajectory::moveOneStepByPVAT(PointPro positionIn, Twist velIn, Twist accIn, Twist torqueIn)
{
    if(NULL!=getTrajComposite())
    {
        return -1;
    }
        positionCommand=positionIn;
        velCommand=velIn;
        accCommand=accIn;
        torqueCommand=torqueIn;

    return isLimitCheckOk();
}


Frame CoordinateTrajectory::dpointToFrame(const DPoint &pointIn)
{
    Frame tmpFrame;

    Vector tmpPos(pointIn.posX/1000.0,pointIn.posY/1000.0,0);
    Rotation tmpRot;
    tmpFrame = Frame(tmpRot.RPY(0,0,pointIn.posR*M_PI/180.0),tmpPos);

    return tmpFrame;

}

Frame CoordinateTrajectory::vehiclePositionToFrame(const VehiclePosition &pointIn)
{
    Frame tmpFrame;
    Vector tmpPos(pointIn.x,pointIn.y,0);
    Rotation tmpRot;
    tmpFrame = Frame(tmpRot.RPY(0,0,pointIn.rotate),tmpPos);
    return tmpFrame;

}


int CoordinateTrajectory::lineMove(Frame positionIn, double speedPercent, double &timeDurationResult,
                                   int cntIn,double minMixDurationIn)
{
//    if(NULL!=getTrajComposite())
//    {
//        qDebug()<<"error,NULL!=trajComposite, CoordinateTrajectory::lineMove";
//        return -1;
//    }

    if(1)
    {
        qDebug()<<"CoordinateTrajectory::lineMove positionFrameCommand_planed"
               <<positionFrameCommand_planed.p[0]<<positionFrameCommand_planed.p[1]<<positionFrameCommand_planed.p[2]
              <<"positionIn"<<positionIn.p[0]<<positionIn.p[1]<<positionIn.p[2];
    }

    // Path_RoundedComposite defines the geometric path along
    // which the robot will move. 可以设置拐角平滑度
    Path_RoundedComposite* path = new Path_RoundedComposite(0.0,rotate_eqradius,
                                                            new RotationalInterpolation_SingleAxis(),true);

    //将但其当前点写入第一个点
    path->Add(positionFrameCommand_planed);
    path->Add(positionIn);
    path->Finish();

    // Trajectory defines a motion of the robot along a path.
    // This defines a trapezoidal velocity profile.
    qDebug()<<"lineMove velocityStype"<<velocityStype;
    VelocityProfile* velpref;
    switch(velocityStype)
    {
    case E_VELOCITY_TYPE_IMPROVED_SPLINE:
    {
        velpref = new VelocityProfile_ImprovedSpline(speedPercent*maxVelocity_line,maxAccelation_line*accelerationRatio);
        break;
    }
    case E_VELOCITY_TYPE_SIN:
    {
        velpref = new VelocityProfile_sin(speedPercent*maxVelocity_line,maxAccelation_line*accelerationRatio);
        break;
    }
    default:
    {
        velpref = new VelocityProfile_Trap(speedPercent*maxVelocity_line,maxAccelation_line*accelerationRatio);
    }
    }


    velpref->SetProfile(0,path->PathLength());
    Trajectory* traject = new Trajectory_Segment(path, velpref);
    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();
//    KDL::Trajectory_Composite* tmpTrajAttri.trajComp =    addNewTrajComposite();;
    tmpTrajAttri.trajComp->Add(traject);

    timeDurationResult=tmpTrajAttri.trajComp->Duration();
    if(timeDurationResult<minMixDurationIn)
    {
        qDebug()<<"line轨迹时间小于Mix过渡时间timeDurationResult"<<timeDurationResult<<"minMixDurationIn"<<minMixDurationIn;
        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6017,robotId,timeDurationResult,minMixDurationIn);
        return  -1;
    }

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=cntIn;
    tmpTrajAttri.timeDuration=timeDurationResult;
    addNewTrajComposite(tmpTrajAttri);
    positionFrameCommand_planed=positionIn;
    return 1;
}


int CoordinateTrajectory::lineMove_3segment_composed(bool isPositiveDirection, bool isVelocitySpline,
                  VehiclePosition positionIn, double speedPercent, double &timeDurationResult, bool isEndRotate)
{
//lastCommand_velocity_line
    if(NULL!=getTrajComposite())
    {
        return -1;
    }

    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();
    SegmentAtrribute tmpAttribute;
    segmentAtrributeQueue.clear();
    //舵轮转角过渡
    double tmpFirstSteeringTargetAngle=steeringPositionFeedback[0];
    if(isSteeringValid)
    {
            Trajectory* tmpStaTraj;
            steeringRotatePlan(tmpFirstSteeringTargetAngle,true,&tmpStaTraj,tmpFirstSteeringTargetAngle);
            if(NULL!=tmpStaTraj)
            {
                tmpTrajAttri.trajComp->Add(tmpStaTraj);
                tmpAttribute.motionId=E_MOTION_ID_MOVE_WAIT_STEERING;
                tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
                segmentAtrributeQueue.append(tmpAttribute);
                qDebug()<<"舵轮过渡tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;
            }


    }

     //step 1 旋转

    //求第一个点，旋转角度点
    double lineAngle=atan2(positionIn.y-vehiclePositionCommand.y,positionIn.x-vehiclePositionCommand.x);
    double rotateFistAngle;//直线段运行方向
    if(true==isPositiveDirection)
    {
        rotateFistAngle=lineAngle;
    }
    else
    {
        rotateFistAngle=lineAngle-M_PI;
        rotateFistAngle=transformToPi(rotateFistAngle);
    }
    VehiclePosition rotationEndPosition;
    rotationEndPosition.x=vehiclePositionCommand.x;
    rotationEndPosition.y=vehiclePositionCommand.y;
    rotationEndPosition.rotate=rotateFistAngle;

    Trajectory * tmpTraj;
    double tmp_moveAngle=fabs(angleRotationJudgement_improved(rotateFistAngle,vehiclePositionCommand.rotate));
    qDebug()<<"step 1　tmp_moveAngle="<<tmp_moveAngle;
    if(tmp_moveAngle>=noRotateMoveAngle)
    {
        splineRotatePlan(isVelocitySpline,vehiclePositionCommand,rotationEndPosition,speedPercent,&tmpTraj);
        tmpTrajAttri.trajComp->Add(tmpTraj);
        tmpAttribute.motionId=E_MOTION_ID_ROTATE;
        tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
        segmentAtrributeQueue.append(tmpAttribute);
        qDebug()<<"tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;
    }
    else
    {
        qDebug()<<"step 1　tmp_moveAngle<"<<noRotateMoveAngle;
    }
    //舵轮过渡
    if(isSteeringValid)
    {
            Trajectory* tmpStaTraj;
            steeringRotatePlan(tmpFirstSteeringTargetAngle,false,&tmpStaTraj,tmpFirstSteeringTargetAngle);
            if(NULL!=tmpStaTraj)
            {
                tmpTrajAttri.trajComp->Add(tmpStaTraj);
                tmpAttribute.motionId=E_MOTION_ID_MOVE_WAIT_STEERING;
                tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
                segmentAtrributeQueue.append(tmpAttribute);
                qDebug()<<"舵轮过渡tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;
            }


    }

    //step 2 直线段
    VehiclePosition lineEndPosition;
    lineEndPosition.x=positionIn.x;
    lineEndPosition.y=positionIn.y;
    lineEndPosition.rotate=rotateFistAngle;
//    QVector<Trajectory *> trajListOut;
//    singleSmoothLinePlan(rotationEndPosition,lineEndPosition,speedPercent,trajListOut);
//    for(int i=0;i<trajListOut.size();i++)
//    {
//        tmpTrajAttri.trajComp->Add(trajListOut[i]);
//    }

    Trajectory *tmpTrajOut;
    improvedSplineLinePlan(E_LINE_VELOCITY_STYLE_ZERO_ZERO,rotationEndPosition,lineEndPosition,speedPercent,&tmpTrajOut);
    tmpTrajAttri.trajComp->Add(tmpTrajOut);

    tmpAttribute.motionId=E_MOTION_ID_MOVE_LINE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);
    qDebug()<<"line tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;


    //step 3 旋转段
    if(isEndRotate)
    {
        tmp_moveAngle=fabs(angleRotationJudgement_improved(rotateFistAngle,positionIn.rotate));
        qDebug()<<"step 3　tmp_moveAngle="<<tmp_moveAngle;
    //    if(tmp_moveAngle>=noRotateMoveAngle)//怀疑是，这个逻辑有问题。聿冠出现到位后，没有旋转，而是直线加速，然后报反馈加速度过快。
    //    {
            //纯粹旋转不能用spline，时间计算不对。
            splineRotatePlan(isVelocitySpline,lineEndPosition,positionIn,speedPercent,&tmpTraj);
            tmpTrajAttri.trajComp->Add(tmpTraj);
            tmpAttribute.motionId=E_MOTION_ID_ROTATE;
            timeDurationResult=tmpTrajAttri.trajComp->Duration();
            tmpAttribute.timeStamp=timeDurationResult;
            segmentAtrributeQueue.append(tmpAttribute);
            qDebug()<<"tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;
    //    }
    //    else
    //    {
    //        qDebug()<<"step 3　tmp_moveAngle<"<<noRotateMoveAngle;
    //    }
    }


    timeDurationResult=tmpTrajAttri.trajComp->Duration();

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=timeDurationResult;
    addNewTrajComposite(tmpTrajAttri);

    return 1;
}


int CoordinateTrajectory::lineMove_maccrum_2segment_composed(bool isVelocitySpline, VehiclePosition positionIn,
                                                             double speedPercent, double &timeDurationResult)
{

    if(NULL!=getTrajComposite())
    {
        return -1;
    }

    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();

     //step 1 旋转
    segmentAtrributeQueue.clear();
    //求第一个点，旋转角度点，直接到达最后点的姿态角
    VehiclePosition rotationEndPosition;
    rotationEndPosition.x=vehiclePositionCommand.x;
    rotationEndPosition.y=vehiclePositionCommand.y;
    rotationEndPosition.rotate=positionIn.rotate;
    SegmentAtrribute tmpAttribute;
    Trajectory * tmpTraj;
    double tmp_moveAngle=fabs(angleRotationJudgement_improved(positionIn.rotate,vehiclePositionCommand.rotate));
    qDebug()<<"step 1　tmp_moveAngle="<<tmp_moveAngle;
    if(tmp_moveAngle>=noRotateMoveAngle)
    {
        splineRotatePlan(isVelocitySpline,vehiclePositionCommand,rotationEndPosition,speedPercent,&tmpTraj);
        tmpTrajAttri.trajComp->Add(tmpTraj);
        tmpAttribute.motionId=E_MOTION_ID_ROTATE;
        tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
        segmentAtrributeQueue.append(tmpAttribute);
    }
    else
    {
        qDebug()<<"step 1　tmp_moveAngle<"<<noRotateMoveAngle;
    }

    //step 2 直线段
    VehiclePosition lineEndPosition;
    lineEndPosition.x=positionIn.x;
    lineEndPosition.y=positionIn.y;
    lineEndPosition.rotate=positionIn.rotate;
    Trajectory *tmpTrajOut;
    improvedSplineLinePlan(E_LINE_VELOCITY_STYLE_ZERO_ZERO,rotationEndPosition,lineEndPosition,speedPercent,&tmpTrajOut);
    tmpTrajAttri.trajComp->Add(tmpTrajOut);
    tmpAttribute.motionId=E_MOTION_ID_MOVE_LINE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);


    timeDurationResult=tmpTrajAttri.trajComp->Duration();

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=timeDurationResult;
    addNewTrajComposite(tmpTrajAttri);

    return 1;
}

int CoordinateTrajectory::bezierMove_noRotateFromCurrentPos(E_LINE_VELOCITY_STYLE velStyle, bool isPositiveDirection,
                                  VehiclePosition positionIn, double speedPercent, double &timeDurationResult)
{
    qDebug()<<"CoordinateTrajectory::bezierMove_noRotateFromCurrentPos";
    if(NULL!=getTrajComposite())
    {
        return 0;
    }

    VehiclePosition startPos, endPos;
    if(isPositiveDirection)
    {
        startPos=vehiclePositionCommand;
        endPos=positionIn;
    }
    else
    {
        startPos=vehiclePositionCommand;
        endPos=positionIn;
        startPos.rotate+=M_PI;
        endPos.rotate+=M_PI;
    }


    SegmentAtrribute tmpAttribute;
    QVector<Trajectory *> trajListOut;
    int tmpKey=bezierMove_noRotate(velStyle,startPos,endPos,speedPercent,trajListOut);
    if(1!=tmpKey)
    {
        qDebug()<<"bezierMove_noRotate failed";
        return -1;
    }



    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();
    segmentAtrributeQueue.clear();

    //舵轮过渡
    double tmpFirstSteeringTargetAngle;
    if(isSteeringValid)
    {
            Trajectory* tmpStaTraj;
            steeringRotatePlan(steeringPositionFeedback[0],false,&tmpStaTraj,tmpFirstSteeringTargetAngle);
            if(NULL!=tmpStaTraj)
            {
                tmpTrajAttri.trajComp->Add(tmpStaTraj);
                tmpAttribute.motionId=E_MOTION_ID_MOVE_WAIT_STEERING;
                tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
                segmentAtrributeQueue.append(tmpAttribute);
                qDebug()<<"舵轮过渡tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;
            }


    }

    //bezier
    for(int i=0;i<trajListOut.size();i++)
    {
       tmpTrajAttri.trajComp->Add(trajListOut[i]);
    }
    tmpAttribute.motionId=E_MOTION_ID_MOVE_LINE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);
    qDebug()<<"line tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;



    timeDurationResult=tmpTrajAttri.trajComp->Duration();

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=timeDurationResult;
    addNewTrajComposite(tmpTrajAttri);

    return 1;
}

int CoordinateTrajectory::bezierMove_withControlPoints(E_LINE_VELOCITY_STYLE velStyle, bool isPositiveDirection,
                                      VehiclePosition targetPositionIn, VehiclePosition pos1, VehiclePosition pos2,
                                                       double speedPercent, double &timeDurationResult)
{
    qDebug()<<"CoordinateTrajectory::bezierMove_withControlPoints";
    if(NULL!=getTrajComposite())
    {
        return 0;
    }


    SegmentAtrribute tmpAttribute;
    QVector<Trajectory *> trajListOut;
    int tmpKey=bezierMove_noRotateWithControlPoint(velStyle,vehiclePositionCommand,targetPositionIn,
                                                   pos1,pos2,speedPercent,trajListOut);
    if(1!=tmpKey)
    {
        qDebug()<<"bezierMove_noRotate failed";
        return -1;
    }



    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();
    segmentAtrributeQueue.clear();

    //舵轮过渡
    double tmpFirstSteeringTargetAngle;
    if(isSteeringValid)
    {
            Trajectory* tmpStaTraj;
            steeringRotatePlan(steeringPositionFeedback[0],false,&tmpStaTraj,tmpFirstSteeringTargetAngle);
            if(NULL!=tmpStaTraj)
            {
                tmpTrajAttri.trajComp->Add(tmpStaTraj);
                tmpAttribute.motionId=E_MOTION_ID_MOVE_WAIT_STEERING;
                tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
                segmentAtrributeQueue.append(tmpAttribute);
                qDebug()<<"舵轮过渡tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;
            }


    }

    //bezier
    for(int i=0;i<trajListOut.size();i++)
    {
       tmpTrajAttri.trajComp->Add(trajListOut[i]);
    }
    tmpAttribute.motionId=E_MOTION_ID_MOVE_LINE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);
    qDebug()<<"line tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;



    timeDurationResult=tmpTrajAttri.trajComp->Duration();

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=timeDurationResult;
    addNewTrajComposite(tmpTrajAttri);

    return 1;
}


int CoordinateTrajectory::bezierMove_noRotate(E_LINE_VELOCITY_STYLE velStyle, VehiclePosition startPos, VehiclePosition endPos,
                                              double speedRatioIn,QVector<Trajectory *> &trajListOut)
{
    trajListOut.clear();
//    VehiclePosition passPos1,passPos2;
//    double lineLength=sqrt((startPos.x-endPos.x)*(startPos.x-endPos.x)
//                             +(startPos.y-endPos.y)*(startPos.y-endPos.y));
//    double lineLength_x=endPos.x-startPos.x;
//    double lineLength_y=endPos.y-startPos.y;

    KDL::Vector startPoint,startPoint_orient, endPoint,endPoint_orient;
    startPoint.data[0]=startPos.x;
    startPoint.data[1]=startPos.y;
    startPoint.data[2]=0;

    endPoint.data[0]=endPos.x;
    endPoint.data[1]=endPos.y;
    endPoint.data[2]=0;

    double totalLength=sqrt((startPos.x-endPos.x)*(startPos.x-endPos.x)+(startPos.y-endPos.y)*(startPos.y-endPos.y));
    double plength=D_BEZIER_LENGTH_RATIO*totalLength;

    startPoint_orient.data[0]=startPos.x+plength*cos(startPos.rotate);
    startPoint_orient.data[1]=startPos.y+plength*sin(startPos.rotate);
    startPoint_orient.data[2]=0;

    endPoint_orient.data[0]=endPos.x+plength*cos(endPos.rotate+M_PI);
    endPoint_orient.data[1]=endPos.y+plength*sin(endPos.rotate+M_PI);
    endPoint_orient.data[2]=0;


    //路径合理性判断.如果开始和停止方向成180度时，角速度估计偏差就很大。所以限制方向小于１００ｄｅgree
    double tmpRotateAngle=angleRotationJudgement_improved(endPos.rotate,startPos.rotate);
    if(fabs(tmpRotateAngle)>D_SPLINE_ALLOW_ANGLE*M_PI/180.0)
    {
        qDebug()<<"fabs(tmpRotateAngle)>100*M_PI/180.0"<<"tmpRotateAngle"<<tmpRotateAngle<<"startPos.rotate"<<startPos.rotate
                  <<"startPos.rotate"<<endPos.rotate;
//        delete path;
//        delete velpref;
        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6010,robotId,D_SPLINE_ALLOW_ANGLE);
        return  -1;
    }

    //路径合理性判断.起点到终点的距离
    double deltaDistance=sqrt((startPos.x-endPos.x)*(startPos.x-endPos.x)+(startPos.y-endPos.y)*(startPos.y-endPos.y));
//    if(deltaDistance<D_SPLINE_ALLOW_DISTANCE)
//    {
//        qDebug()<<"error,,,,deltaDistance"<<deltaDistance;
//    addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6012,robotId,D_SPLINE_ALLOW_DISTANCE);
//        return  -2;
//    }

    //路径合理性判断.。aStart开始姿态到直线方向的夹角，bStart 直线方向到结束姿态的夹角。
    //aStart　bStart必须同正或同负，如果一正一负，则绝对值的和必须小于１０degree
    double tmpLineAngle=atan2(endPos.y-startPos.y,endPos.x-startPos.x);
    double aStart=angleRotationJudgement_improved(tmpLineAngle,startPos.rotate);
    double bStart=angleRotationJudgement_improved(endPos.rotate,tmpLineAngle);
    if((aStart>0 & bStart<0) || (aStart<0 & bStart>0) )
    {
        if(fabs(aStart)>D_SPLINE_ALLOW_CONFILIT_ANGLE*M_PI/180.0 && fabs(bStart)>D_SPLINE_ALLOW_CONFILIT_ANGLE*M_PI/180.0)
        {
            qDebug()<<"error,　姿态过渡角度必须同正或同负，如果一正一负，则绝对值的和必须小于１０degree,aStart"
                   <<aStart<<"bStart"<<bStart<<"tmpLineAngle"<<tmpLineAngle
                     <<"startPos.x"<<startPos.x<<"startPos.y"<<startPos.y<<"startPos.rotate"<<startPos.rotate
                       <<"endPos.x"<<endPos.x<<"endPos.y"<<endPos.y<<"endPos.rotate"<<endPos.rotate;
    //        delete path;
    //        delete velpref;
            addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6015,robotId,D_SPLINE_ALLOW_CONFILIT_ANGLE);
            return  -3;
        }
    }


    //third path
    KDL::Path* path = new KDL::Path_3Bezier(startPoint,startPoint_orient, endPoint,endPoint_orient);
//    path->Add(vehiclePositionToFrame(passPos2));
//    path->Add(vehiclePositionToFrame(endPos));
//    path->Finish();
//    VelocityProfile* velpref= new VelocityProfile_Spline();
//    double tmpTimeDuration=2.6*path->PathLength()/(maxVelocity_line*speedRatioIn);
//    ((VelocityProfile_Spline*)velpref)->SetProfileDuration(0, 0, 0.0, path->PathLength(),
//                                                           0, 0.0, tmpTimeDuration);
    //只能用VelocityProfile_ImprovedSpline，涉及到速度拼接和过渡
    VelocityProfile* velpref= new VelocityProfile_ImprovedSpline(maxVelocity_line*speedRatioIn,maxAccelation_line*accelerationRatio);
    double tmpVelNew;
    switch(velStyle)
    {
    case E_LINE_VELOCITY_STYLE_ZERO_ZERO:
    {
        velpref->SetProfile(0,  path->PathLength());
        lastCommand_velocity_line=0;
        break;
    }
    case E_LINE_VELOCITY_STYLE_CHANGE_CONSTANT:
    {
//        velpref->SetProfile(0,  path->PathLength());
//        lastCommand_velocity_line=0;
//        break;
        tmpVelNew=maxVelocity_line*speedRatioIn;
        ((VelocityProfile_ImprovedSpline*)velpref)->SetProfile_transsition(0,lastCommand_velocity_line,
                                                       path->PathLength(),tmpVelNew,true);
        lastCommand_velocity_line=tmpVelNew;//加速度限制，可能达不到要求的速度。
        break;
    }
    case E_LINE_VELOCITY_STYLE_CONSTANT_CHANGE:
    {
//        velpref->SetProfile(0,  path->PathLength());
//        lastCommand_velocity_line=0;
//        break;
        tmpVelNew=0;
        int tmpKey=((VelocityProfile_ImprovedSpline*)velpref)->SetProfile_transsition(0,lastCommand_velocity_line,
                                                       path->PathLength(),tmpVelNew,false);
        lastCommand_velocity_line=0;//结尾变的，速度为０
        if(1!=tmpKey)
        {
            qDebug()<<"error, 减速停止时，加速度超过限制，请降低前一段的速度或者加长本段的距离";
            addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6011,robotId);
            delete path;
            delete velpref;
            return  -5;
        }
        break;
    }
    default:
    {
        qDebug()<<"error, unknow velocity plan1, lastCommand_velocity_line"<<lastCommand_velocity_line<<"velStyle"<<velStyle;
        delete path;
        delete velpref;
        return  -6;
    }
    }




    //取消角速度检查，使用起来不方便，直接依靠自动检查角度偏差，然后自动降速，或者认为设定，降低速度。
    double rotateVel1=aStart/velpref->Duration();
    double rotateVel2=bStart/velpref->Duration();
    if(fabs(rotateVel1)>D_SPLINE_ALLOW_ANGLE_MOVE_RATIO*maxVelocity_rotate
            || fabs(rotateVel2)>D_SPLINE_ALLOW_ANGLE_MOVE_RATIO*maxVelocity_rotate)
    {
        qDebug()<<"error ,fabs(rotateVel)>D_SPLINE_ALLOW_ANGLE_MOVE_RATIO*maxVelocity_rotate"<<"rotateVel1"<<rotateVel1
               <<"maxVelocity_rotate"<<maxVelocity_rotate <<"rotateVel2"<<rotateVel2;
        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6014,robotId,D_SPLINE_ALLOW_ANGLE_MOVE_RATIO*maxVelocity_rotate);
        delete path;
        delete velpref;
        return  -7;
    }
    Trajectory* traject = new Trajectory_Segment(path, velpref);
    trajListOut.append(traject);

    return 1;
}

int CoordinateTrajectory::bezierMove_noRotateWithControlPoint(E_LINE_VELOCITY_STYLE velStyle,
                               VehiclePosition startPos, VehiclePosition endPos,
                                VehiclePosition pos1, VehiclePosition pos2,
                               double speedRatioIn, QVector<Trajectory *> &trajListOut)
{
    trajListOut.clear();
//    VehiclePosition passPos1,passPos2;
//    double lineLength=sqrt((startPos.x-endPos.x)*(startPos.x-endPos.x)
//                             +(startPos.y-endPos.y)*(startPos.y-endPos.y));
//    double lineLength_x=endPos.x-startPos.x;
//    double lineLength_y=endPos.y-startPos.y;
    double tmpStartAngle,tmpEndAngle;
    tmpStartAngle=atan2(pos1.y-startPos.y,pos1.x-startPos.x);
    tmpEndAngle=atan2(endPos.y-pos2.y,endPos.x-pos2.x);

    KDL::Vector startPoint,startPoint_orient, endPoint,endPoint_orient;
    startPoint.data[0]=startPos.x;
    startPoint.data[1]=startPos.y;
    startPoint.data[2]=0;

    endPoint.data[0]=endPos.x;
    endPoint.data[1]=endPos.y;
    endPoint.data[2]=0;

//    double totalLength=sqrt((startPos.x-endPos.x)*(startPos.x-endPos.x)+(startPos.y-endPos.y)*(startPos.y-endPos.y));
//    double plength=D_BEZIER_LENGTH_RATIO*totalLength;

    startPoint_orient.data[0]=pos1.x;
    startPoint_orient.data[1]=pos1.y;
    startPoint_orient.data[2]=0;

    endPoint_orient.data[0]=pos2.x;
    endPoint_orient.data[1]=pos2.y;
    endPoint_orient.data[2]=0;


    //路径合理性判断.如果开始和停止方向成180度时，角速度估计偏差就很大。所以限制方向小于１００ｄｅgree
    double tmpRotateAngle=angleRotationJudgement_improved(tmpEndAngle,tmpStartAngle);
    if(fabs(tmpRotateAngle)>D_SPLINE_ALLOW_ANGLE*M_PI/180.0)
    {
        qDebug()<<"fabs(tmpRotateAngle)>100*M_PI/180.0"<<"tmpRotateAngle"<<tmpRotateAngle
               <<"startPos.rotate"<<tmpStartAngle
                  <<"startPos.rotate"<<tmpEndAngle;
//        delete path;
//        delete velpref;
        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6010,robotId,D_SPLINE_ALLOW_ANGLE);
        return  -1;
    }

    //路径合理性判断.起点到终点的距离
    double deltaDistance=sqrt((startPos.x-endPos.x)*(startPos.x-endPos.x)+(startPos.y-endPos.y)*(startPos.y-endPos.y));
//    if(deltaDistance<D_SPLINE_ALLOW_DISTANCE)
//    {
//        qDebug()<<"error,,,,deltaDistance"<<deltaDistance;
//    addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6012,robotId,D_SPLINE_ALLOW_DISTANCE);
//        return  -2;
//    }

    //路径合理性判断.。aStart开始姿态到直线方向的夹角，bStart 直线方向到结束姿态的夹角。
    //aStart　bStart必须同正或同负，如果一正一负，则绝对值的和必须小于１０degree
    double tmpLineAngle=atan2(endPos.y-startPos.y,endPos.x-startPos.x);
    double aStart=angleRotationJudgement_improved(tmpLineAngle,tmpStartAngle);
    double bStart=angleRotationJudgement_improved(tmpEndAngle,tmpLineAngle);
    if((aStart>0 & bStart<0) || (aStart<0 & bStart>0) )
    {
        if(fabs(aStart)>D_SPLINE_ALLOW_CONFILIT_ANGLE*M_PI/180.0 && fabs(bStart)>D_SPLINE_ALLOW_CONFILIT_ANGLE*M_PI/180.0)
        {
            qDebug()<<"error,　姿态过渡角度必须同正或同负，如果一正一负，则绝对值的和必须小于１０degree,aStart"
                   <<aStart<<"bStart"<<bStart<<"tmpLineAngle"<<tmpLineAngle
                     <<"startPos.x"<<startPos.x<<"startPos.y"<<startPos.y<<"startPos.rotate"<<tmpStartAngle
                       <<"endPos.x"<<endPos.x<<"endPos.y"<<endPos.y<<"endPos.rotate"<<tmpEndAngle;
    //        delete path;
    //        delete velpref;
            addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6015,robotId,D_SPLINE_ALLOW_CONFILIT_ANGLE);
            return  -3;
        }
    }


    //third path
    KDL::Path* path = new KDL::Path_3Bezier(startPoint,startPoint_orient, endPoint,endPoint_orient);
//    path->Add(vehiclePositionToFrame(passPos2));
//    path->Add(vehiclePositionToFrame(endPos));
//    path->Finish();
//    VelocityProfile* velpref= new VelocityProfile_Spline();
//    double tmpTimeDuration=2.6*path->PathLength()/(maxVelocity_line*speedRatioIn);
//    ((VelocityProfile_Spline*)velpref)->SetProfileDuration(0, 0, 0.0, path->PathLength(),
//                                                           0, 0.0, tmpTimeDuration);
    //只能用VelocityProfile_ImprovedSpline，涉及到速度拼接和过渡
    VelocityProfile* velpref= new VelocityProfile_ImprovedSpline(maxVelocity_line*speedRatioIn,maxAccelation_line*accelerationRatio);
    double tmpVelNew;
    switch(velStyle)
    {
    case E_LINE_VELOCITY_STYLE_ZERO_ZERO:
    {
        velpref->SetProfile(0,  path->PathLength());
        lastCommand_velocity_line=0;
        break;
    }
    case E_LINE_VELOCITY_STYLE_CHANGE_CONSTANT:
    {
//        velpref->SetProfile(0,  path->PathLength());
//        lastCommand_velocity_line=0;
//        break;
        tmpVelNew=maxVelocity_line*speedRatioIn;
        ((VelocityProfile_ImprovedSpline*)velpref)->SetProfile_transsition(0,lastCommand_velocity_line,
                                                       path->PathLength(),tmpVelNew,true);
        lastCommand_velocity_line=tmpVelNew;//加速度限制，可能达不到要求的速度。
        break;
    }
    case E_LINE_VELOCITY_STYLE_CONSTANT_CHANGE:
    {
//        velpref->SetProfile(0,  path->PathLength());
//        lastCommand_velocity_line=0;
//        break;
        tmpVelNew=0;
        int tmpKey=((VelocityProfile_ImprovedSpline*)velpref)->SetProfile_transsition(0,lastCommand_velocity_line,
                                                       path->PathLength(),tmpVelNew,false);
        lastCommand_velocity_line=0;//结尾变的，速度为０
        if(1!=tmpKey)
        {
            qDebug()<<"error, 减速停止时，加速度超过限制，请降低前一段的速度或者加长本段的距离";
            addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6011,robotId);
            delete path;
            delete velpref;
            return  -5;
        }
        break;
    }
    default:
    {
        qDebug()<<"error, unknow velocity plan1, lastCommand_velocity_line"<<lastCommand_velocity_line<<"velStyle"<<velStyle;
        delete path;
        delete velpref;
        return  -6;
    }
    }




    //取消角速度检查，使用起来不方便，直接依靠自动检查角度偏差，然后自动降速，或者认为设定，降低速度。
    double rotateVel1=aStart/velpref->Duration();
    double rotateVel2=bStart/velpref->Duration();
    if(fabs(rotateVel1)>D_SPLINE_ALLOW_ANGLE_MOVE_RATIO*maxVelocity_rotate
            || fabs(rotateVel2)>D_SPLINE_ALLOW_ANGLE_MOVE_RATIO*maxVelocity_rotate)
    {
        qDebug()<<"error ,fabs(rotateVel)>D_SPLINE_ALLOW_ANGLE_MOVE_RATIO*maxVelocity_rotate"<<"rotateVel1"<<rotateVel1
               <<"maxVelocity_rotate"<<maxVelocity_rotate <<"rotateVel2"<<rotateVel2;
        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6014,robotId,D_SPLINE_ALLOW_ANGLE_MOVE_RATIO*maxVelocity_rotate);
        delete path;
        delete velpref;
        return  -7;
    }
    Trajectory* traject = new Trajectory_Segment(path, velpref);
    trajListOut.append(traject);

    return 1;
}

//int CoordinateTrajectory::lineMove_3segment_composed(bool isPositiveDirection, bool isVelocitySpline, VehiclePosition positionIn,
//                                                     double speedPercent, double &timeDurationResult)
//{

//    if(NULL!=trajComposite)
//    {
//        return -1;
//    }

//    Frame tmpPosition=vehiclePositionToFrame(positionIn);

//    trajComposite = new Trajectory_Composite();

//     //step 1
//    segmentAtrributeQueue.clear();
//    Path_RoundedComposite* path = new Path_RoundedComposite(0.0,0.5,new RotationalInterpolation_SingleAxis(),true);

//    //将当前点写入第一个点
//    path->Add(positionFrameCommand);

//    //求第一个点，旋转角度点
//    double lineAngle=atan2(tmpPosition.p[1]-positionFrameCommand.p[1],tmpPosition.p[0]-positionFrameCommand.p[0]);
//    double rotateFistAngle;
//    if(true==isPositiveDirection)
//    {
//        rotateFistAngle=lineAngle;
//    }
//    else
//    {
//        rotateFistAngle=lineAngle-M_PI;
//        rotateFistAngle=transformToPi(rotateFistAngle);
//    }
//    Rotation tmpRot;
////    Vector tmpPos=positionFrameCommand.p;
////    tmpPos[0]+=2;

//    Frame firstFrame = Frame(tmpRot.RPY(0,0,rotateFistAngle),positionFrameCommand.p);
//    path->Add(firstFrame);
//    path->Finish();

//    double angleBias=transformToPi(rotateFistAngle-vehiclePositionCommand.rotate);
//    // Trajectory defines a motion of the robot along a path.
//    // This defines a trapezoidal velocity profile.

//    VelocityProfile* velpref;

//    //旋转加速度和直线加速度不同，单独设置
//    if(true==isVelocitySpline)
//    {
//        velpref= new VelocityProfile_Spline();
//        double tmpTimeDuration=2.6*path->PathLength()/(maxVelocity_line*speedPercent);
//        ((VelocityProfile_Spline*)velpref)->SetProfileDuration(0, 0.0, 0.0, path->PathLength(), 0.0, 0.0, tmpTimeDuration);
//    }
//    else
//    {
//        velpref = new VelocityProfile_Trap(speedPercent*maxVelocity_line,maxAccelation_line);
//        velpref->SetProfile(0,path->PathLength());
//    }

//    Trajectory* traject = new Trajectory_Segment(path, velpref);
//    trajComposite->Add(traject);
//    SegmentAtrribute tmpAttribute;
//    tmpAttribute.motionId=E_MOTION_ID_ROTATE;
//    tmpAttribute.timeStamp=trajComposite->Duration();
//    segmentAtrributeQueue.append(tmpAttribute);


//    //step 2
////    smoothLinePlan();
//    path = new Path_RoundedComposite(0.0,0.01,new RotationalInterpolation_SingleAxis(),true);
//    path->Add(firstFrame);
//    //求第2个点，直线停止点
//    Frame secondFrame = Frame(tmpRot.RPY(0,0,rotateFistAngle),tmpPosition.p);
//    path->Add(secondFrame);
//    path->Finish();

//    // Trajectory defines a motion of the robot along a path.
//    // This defines a trapezoidal velocity profile.
//    if(true==isVelocitySpline)
//    {
//        velpref= new VelocityProfile_Spline();
//        double tmpTimeDuration=2.6*path->PathLength()/(maxVelocity_line*speedPercent);
//        ((VelocityProfile_Spline*)velpref)->SetProfileDuration(0, 0.0, 0.0, path->PathLength(), 0.0, 0.0, tmpTimeDuration);
//    }
//    else
//    {
//        velpref = new VelocityProfile_Trap(speedPercent*maxVelocity_line,maxAccelation_line);
//        velpref->SetProfile(0,path->PathLength());
//    }

//    traject = new Trajectory_Segment(path, velpref);
//    trajComposite->Add(traject);
//    tmpAttribute.motionId=E_MOTION_ID_MOVE_LINE;
//    tmpAttribute.timeStamp=trajComposite->Duration();
//    segmentAtrributeQueue.append(tmpAttribute);


//    //step 3
//    if(fabs(rotateFistAngle-positionIn.rotate)>2)
//    {
//        path = new Path_RoundedComposite(0.0,0.5,new RotationalInterpolation_SingleAxis(),true);
//        path->Add(secondFrame);
//        //直线末端旋转点
//        path->Add(tmpPosition);
//        path->Finish();

//        angleBias=transformToPi(rotateFistAngle-fromFrame(tmpPosition).rotate);
//        // Trajectory defines a motion of the robot along a path.
//        // This defines a trapezoidal velocity profile.

//        if(true==isVelocitySpline)
//        {
//            velpref= new VelocityProfile_Spline();
//            double tmpTimeDuration=2.6*path->PathLength()/(maxVelocity_line*speedPercent);
//            ((VelocityProfile_Spline*)velpref)->SetProfileDuration(0, 0.0, 0.0, path->PathLength(), 0.0, 0.0, tmpTimeDuration);
//        }
//        else
//        {
//            velpref = new VelocityProfile_Trap(speedPercent*maxVelocity_line,maxAccelation_line);
//            velpref->SetProfile(0,path->PathLength());
//        }


//        traject = new Trajectory_Segment(path, velpref);
//        trajComposite->Add(traject);
//        tmpAttribute.motionId=E_MOTION_ID_ROTATE;
//        timeDurationResult=trajComposite->Duration();
//        tmpAttribute.timeStamp=timeDurationResult;
//        segmentAtrributeQueue.append(tmpAttribute);
//    }

//    timeDurationResult=trajComposite->Duration();
//    accumulatedTime=0;

//    return 1;
//}

int CoordinateTrajectory::lineMove_only_rotation(bool isVelocitySpline, VehiclePosition positionIn,
                                                 double speedPercent, double &timeDurationResult)
{
    if(NULL!=getTrajComposite())
    {
        return -1;
    }

    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();
    SegmentAtrribute tmpAttribute;
    segmentAtrributeQueue.clear();
    //舵轮过渡
    double tmpFirstSteeringTargetAngle;
    if(isSteeringValid)
    {
            Trajectory* tmpStaTraj;
            steeringRotatePlan(steeringPositionFeedback[0],true,&tmpStaTraj,tmpFirstSteeringTargetAngle);
            if(NULL!=tmpStaTraj)
            {
                tmpTrajAttri.trajComp->Add(tmpStaTraj);
                tmpAttribute.motionId=E_MOTION_ID_MOVE_WAIT_STEERING;
                tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
                segmentAtrributeQueue.append(tmpAttribute);
                qDebug()<<"舵轮过渡tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;
            }


    }


     //step 1

    Trajectory * tmpTraj;
    splineRotatePlan(isVelocitySpline,vehiclePositionCommand,positionIn,speedPercent,&tmpTraj);
    tmpTrajAttri.trajComp->Add(tmpTraj);

    tmpAttribute.motionId=E_MOTION_ID_ROTATE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);

    timeDurationResult=tmpTrajAttri.trajComp->Duration();

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=timeDurationResult;
    addNewTrajComposite(tmpTrajAttri);

    return 1;
}

int CoordinateTrajectory::lineMove_4segment_composed(bool isPositiveDirection, Frame positionIn, double speedPercent,
                                                     double &timeDurationResult)
{

    if(NULL!=getTrajComposite())
    {
        return -1;
    }

    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();

     //step 1
    segmentAtrributeQueue.clear();
    Path_RoundedComposite* path = new Path_RoundedComposite(0.0,0.5,new RotationalInterpolation_SingleAxis(),true);

    //将当前点写入第一个点
    path->Add(positionFrameCommand);

    //求第一个点，旋转角度点
    double lineAngle=atan2(positionIn.p[1]-positionFrameCommand.p[1],positionIn.p[0]-positionFrameCommand.p[0]);
    double rotateFistAngle;
    if(true==isPositiveDirection)
    {
        rotateFistAngle=lineAngle;
    }
    else
    {
        rotateFistAngle=lineAngle-M_PI;
        rotateFistAngle=transformToPi(rotateFistAngle);
    }
    Rotation tmpRot;
//    Vector tmpPos=positionFrameCommand.p;
//    tmpPos[0]+=2;

    Frame firstFrame = Frame(tmpRot.RPY(0,0,rotateFistAngle),positionFrameCommand.p);
    path->Add(firstFrame);
    path->Finish();

    double angleBias=transformToPi(rotateFistAngle-vehiclePositionCommand.rotate);
    // Trajectory defines a motion of the robot along a path.
    // This defines a trapezoidal velocity profile.
    VelocityProfile* velpref = new VelocityProfile_Trap(speedPercent*maxVelocity_line,maxAccelation_line*accelerationRatio);//旋转加速度和直线加速度不同，单独设置
    velpref->SetProfile(0,path->PathLength());
    Trajectory* traject = new Trajectory_Segment(path, velpref);
    tmpTrajAttri.trajComp->Add(traject);
    SegmentAtrribute tmpAttribute;
    tmpAttribute.motionId=E_MOTION_ID_ROTATE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);


    //step 2
    path = new Path_RoundedComposite(0.0,0.01,new RotationalInterpolation_SingleAxis(),true);
    path->Add(firstFrame);
    //求第2个点，直线停止点
    Frame secondFrame = Frame(tmpRot.RPY(0,0,rotateFistAngle),positionIn.p);
    path->Add(secondFrame);
    path->Finish();

    // Trajectory defines a motion of the robot along a path.
    // This defines a trapezoidal velocity profile.
    velpref = new VelocityProfile_Trap(speedPercent*maxVelocity_line,maxAccelation_line*accelerationRatio);
    velpref->SetProfile(0,path->PathLength());
    traject = new Trajectory_Segment(path, velpref);
    tmpTrajAttri.trajComp->Add(traject);
    tmpAttribute.motionId=E_MOTION_ID_MOVE_LINE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);


    //step 3
    path = new Path_RoundedComposite(0.0,0.5,new RotationalInterpolation_SingleAxis(),true);
    path->Add(secondFrame);
    //直线末端旋转点
    path->Add(positionIn);
    path->Finish();

    angleBias=transformToPi(rotateFistAngle-transferFrameToVehiclePosition(positionIn).rotate);
    // Trajectory defines a motion of the robot along a path.
    // This defines a trapezoidal velocity profile.
    velpref = new VelocityProfile_Trap(speedPercent*maxVelocity_line,maxAccelation_line*accelerationRatio);
    velpref->SetProfile(0,path->PathLength());
    traject = new Trajectory_Segment(path, velpref);
    tmpTrajAttri.trajComp->Add(traject);
    timeDurationResult=tmpTrajAttri.trajComp->Duration();
    tmpAttribute.motionId=E_MOTION_ID_ROTATE;
    tmpAttribute.timeStamp=timeDurationResult;
    segmentAtrributeQueue.append(tmpAttribute);

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=timeDurationResult;
    addNewTrajComposite(tmpTrajAttri);

    return 1;
}

int CoordinateTrajectory::lineContinuousMove_mixedCNT(QVector<Frame> positionIn,
                             QVector<double> speedPercent, QVector<double> &timeDurationResult,
                           QVector<double> cntList,double minMixDurationIn,QVector<int> accListIn)
{
    if(NULL!=getTrajComposite())
    {
        return -1;
    }

    QVector<double> tmpDurationList;
    tmpDurationList.resize(positionIn.size());
    timeDurationResult.resize(positionIn.size());

    for(int i=0;i<positionIn.size();i++)
    {
        if(accListIn[i]>0)
        {
            setAccelerationRatio(accListIn[i]/100.0);
        }

        lineMove(positionIn[i],speedPercent[i],tmpDurationList[i],cntList[i]);
        positionFrameCommand=positionIn[i];

    }
    //求cut时间list
    QVector<double> tmpCntCutList;
    for(int i=0;i<positionIn.size()-1;i++)
    {
            double tmpMinTime;
            if(0==i)
            {
                if(tmpDurationList[i]<tmpDurationList[i+1]/2.0)
                {
                    tmpMinTime=tmpDurationList[i];
                }
                else
                {
                    tmpMinTime=tmpDurationList[i+1]/2.0;
                }
                double tmpCutTime=tmpMinTime*cntList[i]/100.0;
                tmpCntCutList.append(tmpCutTime);
            }
            else if(positionIn.size()-2==i)
            {
                if(tmpDurationList[i]/2.0<tmpDurationList[i+1])
                {
                    tmpMinTime=tmpDurationList[i]/2.0;
                }
                else
                {
                    tmpMinTime=tmpDurationList[i+1];
                }
                double tmpCutTime=tmpMinTime*cntList[i]/100.0;
                tmpCntCutList.append(tmpCutTime);
            }
            else
            {
                if(tmpDurationList[i]<tmpDurationList[i+1])
                {
                    tmpMinTime=tmpDurationList[i];
                }
                else
                {
                    tmpMinTime=tmpDurationList[i+1];
                }
                double tmpCutTime=tmpMinTime*cntList[i]/100.0/2.0;
                tmpCntCutList.append(tmpCutTime);
            }

    }
    tmpCntCutList.append(0);


    double tmpFrontCntTime=0;
    double tmpBackCntTime=0;
    for(int i=0;i<positionIn.size();i++)
    {
        if(0==i)
        {
            tmpFrontCntTime=0;
            tmpBackCntTime=tmpCntCutList[i];
        }
        else if(positionIn.size()-1==i)
        {
            tmpBackCntTime=0;
            tmpFrontCntTime=tmpCntCutList[i-1];
        }
        else
        {
            tmpFrontCntTime=tmpCntCutList[i-1];
            tmpBackCntTime=tmpCntCutList[i];
        }

        timeDurationResult[i]=tmpDurationList[i]-tmpBackCntTime/2.0;//只减去后面，方便行号更新计算。

    }
    setTrajCompositeEndCutoffTime(tmpCntCutList);
    qDebug()<<"tmpDurationList"<<tmpDurationList<<"tmpCntCutList"<<tmpCntCutList<<"cntList"<<cntList
              <<"timeDurationResult"<<timeDurationResult;
    double tmpSumTime=0;
    for(int i=0;i<tmpDurationList.size();i++)
    {
        tmpSumTime+=tmpDurationList[i];
    }

    for(int i=0;i<tmpCntCutList.size();i++)
    {
        tmpSumTime-=tmpCntCutList[i];
    }

    if(tmpSumTime<minMixDurationIn)
    {
        qDebug()<<"line-cnt轨迹时间小于Mix过渡时间 tmpSumTime"<<tmpSumTime<<"minMixDurationIn"<<minMixDurationIn;
        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6017,robotId,tmpSumTime,minMixDurationIn);
        return  -1;
    }

    return 1;


}



int CoordinateTrajectory::lineContinuousMove(QVector<Frame> positionIn, QVector<double> speedPercent
                                  , QVector<double> &timeDurationResult,double cnt)
{
    if(NULL!=getTrajComposite())
    {
        return -1;
    }

    int pointCount=positionIn.size();

    // which the robot will move. 可以设置拐角平滑度
    if(cnt<=0.000001)
    {
        qDebug()<<"lineContinuousMove error, cnt<=0.000001 ";
        return -1;
    }
    Path_RoundedComposite* path = new Path_RoundedComposite(cnt/1000.0,rotate_eqradius
                                                   ,new RotationalInterpolation_SingleAxis(),true);

    try
    {
            //将但其当前点写入第一个点
            path->Add(positionFrameCommand);
            for(int i=0;i<pointCount;i++)
            {
                path->Add(positionIn[i]);
            }
            path->Finish();
    }
    catch(Error_MotionPlanning_Not_Feasible reason)
    {
        int errorCode=reason.GetType();

        switch(errorCode)
        {

            case 3101:
            {
                qDebug()<<"lineContinuousMove error, eq. radius <= 0 "<<errorCode;
                addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6001,robotId);
                break;
            }
            case 3102:
            {
                qDebug()<<"lineContinuousMove error,the first segment in a rounding has zero length "<<errorCode;
                addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6002,robotId);
                break;
            }
            case 3103:
            {
                qDebug()<<"lineContinuousMove error,the second segment in a rounding has zero length "<<errorCode;
                addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6003,robotId);
                break;
            }
            case 3104:
            {
                qDebug()<<"lineContinuousMove error,the angle between the first and the second segment is close to M_PI "<<errorCode;
                addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6004,robotId);
                break;
            }
            case 3105:
            {
                qDebug()<<"lineContinuousMove error,the distance needed for the rounding is larger then the first segment "<<errorCode;
                addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6005,robotId,cnt);
                break;
            }
            case 3106:
            {
                qDebug()<<"lineContinuousMove error,the distance needed for the rounding is larger then the second segment "<<errorCode;
                addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6006,robotId);
                break;
            }

        }
        delete path;
        return -1;

    }

    // Trajectory defines a motion of the robot along a path.
    // This defines a trapezoidal velocity profile.
    qDebug()<<"lineContinuousMove velocityStype"<<velocityStype;
    VelocityProfile* velpref;
    switch(velocityStype)
    {
    case E_VELOCITY_TYPE_IMPROVED_SPLINE:
    {
        velpref = new VelocityProfile_ImprovedSpline(speedPercent[pointCount-1]*maxVelocity_line,maxAccelation_line*accelerationRatio);
        break;
    }
    case E_VELOCITY_TYPE_SIN:
    {
        velpref = new VelocityProfile_sin(speedPercent[pointCount-1]*maxVelocity_line,maxAccelation_line*accelerationRatio);
        break;
    }
    default:
    {
        velpref = new VelocityProfile_Trap(speedPercent[pointCount-1]*maxVelocity_line,maxAccelation_line*accelerationRatio);
    }
    }



    velpref->SetProfile(0,path->PathLength());
    Trajectory* traject = new Trajectory_Segment(path, velpref);
    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();
    tmpTrajAttri.trajComp->Add(traject);
    timeDurationResult.resize(pointCount);
    for(int i=0;i<pointCount-1;i++)
    {
        timeDurationResult[i]=0;
        //kdl里面可以获取每个小段的长度，通过名义长度，获取当前的小段序号和小段内部长度。
    }
    timeDurationResult[pointCount-1]=tmpTrajAttri.trajComp->Duration();//没有求出每小段的时间，导致无法知道运行在哪小段。

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=timeDurationResult[pointCount-1];
    addNewTrajComposite(tmpTrajAttri);
    //hualei001利用轨迹长度进行比较。１）存储path每个小段的长度。２）根据velpref速度求ｔ时刻的长度。３）比较长度，判断运行在哪个小段。
    return 1;
}

int CoordinateTrajectory::lineContinuousMove_vehiclePosition(bool isPositiveDirection, bool isVelocitySpline,
                                                             QVector<VehiclePosition> positionIn,
                                                             QVector<double> speedPercent,
                                                             QVector<double> &timeDurationResult, QVector<double> cnt)
{

    if(NULL!=getTrajComposite() || 0==positionIn.size() || 0==speedPercent.size()|| 0==cnt.size())
    {
        return -1;
    }
    else if(positionIn.size()<2)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6008,robotId);
        return -2;
    }


    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();


     //step 1 旋转
    segmentAtrributeQueue.clear();
    //求第一个点，旋转角度点
    double lineAngle=atan2(positionIn[0].y-vehiclePositionCommand.y,positionIn[0].x-vehiclePositionCommand.x);
    double rotateFistAngle;
    if(true==isPositiveDirection)
    {
        rotateFistAngle=lineAngle;
    }
    else
    {
        rotateFistAngle=lineAngle-M_PI;
        rotateFistAngle=transformToPi(rotateFistAngle);
    }
    VehiclePosition rotationEndPosition;
    rotationEndPosition.x=vehiclePositionCommand.x;
    rotationEndPosition.y=vehiclePositionCommand.y;
    rotationEndPosition.rotate=rotateFistAngle;
    SegmentAtrribute tmpAttribute;
    Trajectory * tmpTraj;
    if(fabs(angleRotationJudgement_improved(rotateFistAngle,vehiclePositionCommand.rotate))>=noRotateMoveAngle)
    {
        splineRotatePlan(isVelocitySpline,vehiclePositionCommand,rotationEndPosition,speedPercent[0],&tmpTraj);
        tmpTrajAttri.trajComp->Add(tmpTraj);
        tmpAttribute.motionId=E_MOTION_ID_ROTATE;
        tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
        segmentAtrributeQueue.append(tmpAttribute);
    }

    //step 2 直线段
    QVector<Trajectory *> trajListOut;
    multiLineSmoothLinePlan(rotationEndPosition,positionIn,speedPercent[0],cnt[0],trajListOut);
    for(int i=0;i<trajListOut.size();i++)
    {
       tmpTrajAttri.trajComp->Add(trajListOut[i]);
    }
    tmpAttribute.motionId=E_MOTION_ID_MOVE_LINE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);


    //step 3 旋转段
    //求第末点，旋转角度点
    double lineEndAngle=atan2(positionIn.last().y-positionIn[positionIn.size()-2].y,
                              positionIn.last().x-positionIn[positionIn.size()-2].x);
    rotateFistAngle;
    if(true==isPositiveDirection)
    {
        rotateFistAngle=lineEndAngle;
    }
    else
    {
        rotateFistAngle=lineEndAngle-M_PI;
        rotateFistAngle=transformToPi(rotateFistAngle);
    }

    VehiclePosition lineEndPosition;
    lineEndPosition.x=positionIn.last().x;
    lineEndPosition.y=positionIn.last().y;
    lineEndPosition.rotate=rotateFistAngle;

    if(fabs(angleRotationJudgement_improved(rotateFistAngle,positionIn.last().rotate))>=noRotateMoveAngle)
    {
        //纯粹旋转不能用spline，时间计算不对。
        splineRotatePlan(isVelocitySpline,lineEndPosition,positionIn.last(),speedPercent[0],&tmpTraj);
        tmpTrajAttri.trajComp->Add(tmpTraj);
        tmpAttribute.motionId=E_MOTION_ID_ROTATE;
        tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
        segmentAtrributeQueue.append(tmpAttribute);
    }
    timeDurationResult.clear();
    for(int i=0;i<positionIn.size()-1;i++)
    {
        timeDurationResult.append(0);
    }
    timeDurationResult.append(tmpTrajAttri.trajComp->Duration());
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=timeDurationResult.back();
    addNewTrajComposite(tmpTrajAttri);

    return 1;
}

int CoordinateTrajectory::lineContinuousMove_vehiclePosition_maccrum(bool isVelocitySpline,
                         QVector<VehiclePosition> positionIn, QVector<double> speedPercent,
                                    QVector<double> &timeDurationResult, QVector<double> cnt)
{

    if(NULL!=getTrajComposite() || 0==positionIn.size() || 0==speedPercent.size()|| 0==cnt.size())
    {
        return -1;
    }
    else if(positionIn.size()<2)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6008,robotId);
        return -2;
    }


    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();

     //step 1 旋转
    segmentAtrributeQueue.clear();
    //求第一个点，旋转角度点.直接到达最后点的姿态角
    VehiclePosition rotationEndPosition;
    rotationEndPosition.x=vehiclePositionCommand.x;
    rotationEndPosition.y=vehiclePositionCommand.y;
    rotationEndPosition.rotate=positionIn.last().rotate;
    SegmentAtrribute tmpAttribute;
    Trajectory * tmpTraj;
    if(fabs(angleRotationJudgement_improved(vehiclePositionCommand.rotate,vehiclePositionCommand.rotate))>=noRotateMoveAngle)
    {
        splineRotatePlan(isVelocitySpline,vehiclePositionCommand,rotationEndPosition,speedPercent[0],&tmpTraj);
        tmpTrajAttri.trajComp->Add(tmpTraj);
        tmpAttribute.motionId=E_MOTION_ID_ROTATE;
        tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
        segmentAtrributeQueue.append(tmpAttribute);
    }

    //step 2 直线段
    QVector<Trajectory *> trajListOut;
    multiLineSmoothLinePlan(rotationEndPosition,positionIn,speedPercent[0],cnt[0],trajListOut);
    for(int i=0;i<trajListOut.size();i++)
    {
       tmpTrajAttri.trajComp->Add(trajListOut[i]);
    }
    tmpAttribute.motionId=E_MOTION_ID_MOVE_LINE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);


    timeDurationResult.clear();
    for(int i=0;i<positionIn.size()-1;i++)
    {
        timeDurationResult.append(0);
    }
    timeDurationResult.append(tmpTrajAttri.trajComp->Duration());

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=timeDurationResult.back();
    addNewTrajComposite(tmpTrajAttri);
    return 1;
}

int CoordinateTrajectory::splineContinuousMove_vehiclePosition(bool isPositiveDirection, bool isVelocitySpline,
      QVector<VehiclePosition> positionIn, QVector<double> speedPercent,
                                      QVector<double> &timeDurationResult, QVector<double> cnt)
{
    if(NULL!=getTrajComposite() || 0==positionIn.size() || 0==speedPercent.size()|| 0==cnt.size())
    {
        return -1;
    }
    else if(positionIn.size()<2)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6008,robotId);
        return -2;
    }

    SegmentAtrribute tmpAttribute;
    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();
    segmentAtrributeQueue.clear();
    timeDurationResult.clear();

    //舵轮过渡
    double tmpFirstSteeringTargetAngle;
    if(isSteeringValid)
    {
            Trajectory* tmpStaTraj;
            steeringRotatePlan(steeringPositionFeedback[0],false,&tmpStaTraj,tmpFirstSteeringTargetAngle);
            if(NULL!=tmpStaTraj)
            {
                tmpTrajAttri.trajComp->Add(tmpStaTraj);
                tmpAttribute.motionId=E_MOTION_ID_MOVE_WAIT_STEERING;
                tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
                segmentAtrributeQueue.append(tmpAttribute);
                qDebug()<<"舵轮过渡tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;
            }


    }

    //step 1 直线段

    QVector<Trajectory *> trajListOut;
    E_LINE_VELOCITY_STYLE velStyle;
    VehiclePosition startPos,endPos;
//    double lastSegmentDuration;
    for(int k=0;k<positionIn.size();k++)
    {
        if(0==k)
        {
            velStyle=E_LINE_VELOCITY_STYLE_CHANGE_CONSTANT;
            startPos=vehiclePositionFeedback;

        }
        else if(positionIn.size()-1!=k)
        {
            velStyle=E_LINE_VELOCITY_STYLE_CHANGE_CONSTANT;
            startPos=positionIn[k-1];
        }
        else
        {
            velStyle=E_LINE_VELOCITY_STYLE_CONSTANT_CHANGE;
            startPos=positionIn[k-1];
        }
        endPos=positionIn[k];
        if(false==isPositiveDirection)
        {
            startPos.rotate+=M_PI;
            endPos.rotate+=M_PI;
        }
        double deltaDistance=sqrt((startPos.x-endPos.x)*(startPos.x-endPos.x)+(startPos.y-endPos.y)*(startPos.y-endPos.y));
        if(deltaDistance<D_SPLINE_ALLOW_DISTANCE)
        {
            qDebug()<<"WARNNING ,,,,deltaDistance"<<deltaDistance;

        }
        else
        {
            int tmpKey=bezierMove_noRotate(velStyle, startPos,endPos, speedPercent[k],trajListOut);
            if(1!=tmpKey)
            {
                qDebug()<<"bezierMove_noRotate failed";
                delete tmpTrajAttri.trajComp;
                segmentAtrributeQueue.clear();
                return -1;
            }

            for(int i=0;i<trajListOut.size();i++)
            {
               tmpTrajAttri.trajComp->Add(trajListOut[i]);
            }

        }


        timeDurationResult.append(tmpTrajAttri.trajComp->Duration());
        qDebug()<<"tmpTrajAttri.trajComp->Duration()"<<tmpTrajAttri.trajComp->Duration();

    }
    qDebug()<<"timeDurationResult"<<timeDurationResult;

    tmpAttribute.motionId=E_MOTION_ID_MOVE_LINE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=0;
    for(int i=0;i<timeDurationResult.size();i++)
    {
        tmpTrajAttri.timeDuration+=timeDurationResult[i];
    }

    addNewTrajComposite(tmpTrajAttri);

    return 1;
}

int CoordinateTrajectory::splineContinuousMove_vehiclePositionWithControlPoints(bool isPositiveDirection,
                               bool isVelocitySpline, QVector<VehiclePosition> positionIn,
                     QVector<VehiclePosition> middlePos1, QVector<VehiclePosition> middlePos2,
                     QVector<double> speedPercent, QVector<double> &timeDurationResult, QVector<double> cnt)
{
    if(NULL!=getTrajComposite() || 0==positionIn.size() || 0==speedPercent.size()|| 0==cnt.size())
    {
        return -1;
    }
    else if(positionIn.size()<2)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6008,robotId);
        return -2;
    }

    SegmentAtrribute tmpAttribute;
    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();
    segmentAtrributeQueue.clear();
    timeDurationResult.clear();

    //舵轮过渡
    double tmpFirstSteeringTargetAngle;
    if(isSteeringValid)
    {
            Trajectory* tmpStaTraj;
            steeringRotatePlan(steeringPositionFeedback[0],false,&tmpStaTraj,tmpFirstSteeringTargetAngle);
            if(NULL!=tmpStaTraj)
            {
                tmpTrajAttri.trajComp->Add(tmpStaTraj);
                tmpAttribute.motionId=E_MOTION_ID_MOVE_WAIT_STEERING;
                tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
                segmentAtrributeQueue.append(tmpAttribute);
                qDebug()<<"舵轮过渡tmpAttribute.timeStamp="<<tmpAttribute.timeStamp;
            }


    }

    //step 1 直线段

    QVector<Trajectory *> trajListOut;
    E_LINE_VELOCITY_STYLE velStyle;
    VehiclePosition startPos,endPos,middle1,middle2;
//    double lastSegmentDuration;
    for(int k=0;k<positionIn.size();k++)
    {
        middle1.x=middlePos1[k].x;
        middle1.y=middlePos1[k].y;
        middle2.x=middlePos2[k].x;
        middle2.y=middlePos2[k].y;

        if(0==k)
        {
            velStyle=E_LINE_VELOCITY_STYLE_CHANGE_CONSTANT;
            startPos=vehiclePositionFeedback;

        }
        else if(positionIn.size()-1!=k)
        {
            velStyle=E_LINE_VELOCITY_STYLE_CHANGE_CONSTANT;
            startPos=positionIn[k-1];
        }
        else
        {
            velStyle=E_LINE_VELOCITY_STYLE_CONSTANT_CHANGE;
            startPos=positionIn[k-1];
        }
        endPos=positionIn[k];
        if(false==isPositiveDirection)
        {
            startPos.rotate+=M_PI;
            endPos.rotate+=M_PI;
        }
        double deltaDistance=sqrt((startPos.x-endPos.x)*(startPos.x-endPos.x)+(startPos.y-endPos.y)*(startPos.y-endPos.y));
        if(deltaDistance<D_SPLINE_ALLOW_DISTANCE)
        {
            qDebug()<<"WARNNING ,,,,deltaDistance"<<deltaDistance;

        }
        else
        {
            int tmpKey=bezierMove_noRotateWithControlPoint(velStyle, startPos,endPos,middle1,middle2,
                                                           speedPercent[k],trajListOut);
            if(1!=tmpKey)
            {
                qDebug()<<"bezierMove_noRotate failed";
                delete tmpTrajAttri.trajComp;
                segmentAtrributeQueue.clear();
                return -1;
            }

            for(int i=0;i<trajListOut.size();i++)
            {
               tmpTrajAttri.trajComp->Add(trajListOut[i]);
            }

        }


        timeDurationResult.append(tmpTrajAttri.trajComp->Duration());
        qDebug()<<"tmpTrajAttri.trajComp->Duration()"<<tmpTrajAttri.trajComp->Duration();

    }
    qDebug()<<"continuous spline  timeDurationResult"<<timeDurationResult;

    tmpAttribute.motionId=E_MOTION_ID_MOVE_LINE;
    tmpAttribute.timeStamp=tmpTrajAttri.trajComp->Duration();
    segmentAtrributeQueue.append(tmpAttribute);

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=0;
    for(int i=0;i<timeDurationResult.size();i++)
    {
        tmpTrajAttri.timeDuration+=timeDurationResult[i];
    }
    addNewTrajComposite(tmpTrajAttri);
    qDebug()<<"continuous spline 总共timeDurationResult"<<tmpTrajAttri.timeDuration;

    return 1;
}

int CoordinateTrajectory::splineContinuousMove_vehiclePosition_maccrum(bool isVelocitySpline, QVector<VehiclePosition> positionIn,
                       QVector<double> speedPercent, QVector<double> &timeDurationResult, QVector<double> cnt)
{
    //todo
}


int CoordinateTrajectory::circleMove(QVector<Frame>  positionIn, double speedPercent, QVector<double> &timeDurationResult)
{

//    if(NULL!=getTrajComposite())
//    {
//        return -1;
//    }

    double eps = 1e-6;//20170222华磊修改,原为1E-7
//    double eqradius=0.3;//姿态插补等效半径
    Vector tmp1(positionFrameCommand_planed.p - positionIn[0].p);//第一个向量，从第一个点到初始点
    Vector tmp2(positionIn[1].p - positionIn[0].p);//第二个向量，从第一个点到第二个点
    double l1 = tmp1.Normalize();//单位化第一个向量，并求长度
    double l2 = tmp2.Normalize();//单位化第二个向量，并求长度
    double angle = acos( dot(tmp1,tmp2) );//中间点的夹角

    if( ( l1 < eps ) || (l2 < eps) || ((M_PI-angle) < eps) || (angle < eps) )//有些点重合或者在同一直线上
    {

        if( angle < eps )
        {
            qDebug()<<"circleMove error,angle < eps ";
        }
        else
        {
            qDebug()<<"circleMove error,angle >=eps ";
        }

        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6000,robotId);
        return -1;
    }
    Vector center = tmp1 * tmp2; //圆心，初始化值为圆的法向量
//    if(IF_DEBUG)//测试圆法向量求解是否正确
//    {
//        std::cout << "noramal vector multiple point on circle:" << std::endl;
//        std::cout << dot(center, positionFrameCommand_planed.p) << " vs ";
//        std::cout << dot(center, f1.p) << " vs ";
//        std::cout << dot(center, f2.p) << std::endl;
//    }
    Matrix3d A;
    Vector3d b,c;
    A << center.data[0], center.data[1], center.data[2],
            tmp1.data[0], tmp1.data[1], tmp1.data[2],
            tmp2.data[0], tmp2.data[1], tmp2.data[2];
    b<< dot(center, positionFrameCommand_planed.p), dot(positionFrameCommand_planed.p+positionIn[0].p, positionFrameCommand_planed.p-positionIn[0].p)/
            (2.0*l1), dot(positionIn[1].p+positionIn[0].p, positionIn[1].p-positionIn[0].p)/(2.0*l2);
    c =A.inverse()*b;
    center.data[0] = c(0);
    center.data[1] = c(1);
    center.data[2] = c(2);
    Vector r1(positionIn[0].p - center);
    double radius = r1.Norm(); //半径 kdlerror
//    double radius=100;
    Vector r2(positionIn[1].p - center);
//    if(IF_DEBUG)//测试圆心求解是否正确
//    {
//        Vector r0 = positionFrameCommand_planed.p - center;
//        std::cout << "radius:";
//        std::cout << radius << " vs ";
//        std::cout << r0.Norm() << " vs ";
//        std::cout << r2.Norm() << std::endl;
//    }
    double rad1= 2.0*asin(l1/2.0/radius);//第一段弧度
    double rad2 = 2.0*asin(l2/2.0/radius);//第二段弧度
    Vector tmp3(positionIn[1].p - positionFrameCommand_planed.p);
    tmp3.Normalize();
    if( dot(tmp2,tmp3)<0 )//优弧判断
    {rad1=2.0*M_PI-rad1;}
    else if (dot(tmp1,tmp3)>0)
    {rad2=2.0*M_PI-rad2;}
    Path_Composite* pathC = new Path_Composite();

    if(rad1>M_PI+eps)//第一段是优弧
    {
        pathC->Add(new Path_Circle(positionFrameCommand_planed, center, center-r1, positionIn[0].M, rad1,
                new RotationalInterpolation_SingleAxis(), rotate_eqradius));
    }
    else if(rad1>M_PI-eps)//接近半圆
    {
        pathC->Add(new Path_Circle(positionFrameCommand_planed, center, center-r2, positionIn[0].M, rad1,
                new RotationalInterpolation_SingleAxis(), rotate_eqradius));
    }
    else
    {
        pathC->Add(new Path_Circle(positionFrameCommand_planed, center, positionIn[0].p, positionIn[0].M, rad1,
                new RotationalInterpolation_SingleAxis(), rotate_eqradius));
    }
//    if(IF_DEBUG)
//    {
//        std::cout << "path length:" << pathC->PathLength() << std::endl;
//        std::cout << "f1:" << f1 << std::endl;\
//        std::cout << "circle1 end:" << pathC->Pos(pathC->PathLength()) << std::endl;
//    }
    if(rad2>M_PI+eps)//第二段是优弧
    {pathC->Add(new Path_Circle(positionIn[0], center, center-r2, positionIn[1].M, rad2,
                new RotationalInterpolation_SingleAxis(), rotate_eqradius));}
    else if(rad2>M_PI-eps)//接近半圆
    {pathC->Add(new Path_Circle(positionIn[0], center, center-r1, positionIn[1].M, rad2,
                new RotationalInterpolation_SingleAxis(), rotate_eqradius));}
    else
    {pathC->Add(new Path_Circle(positionIn[0], center, positionIn[1].p, positionIn[1].M, rad2,
                new RotationalInterpolation_SingleAxis(), rotate_eqradius));}

    VelocityProfile* velpref = new VelocityProfile_Trap(speedPercent*maxVelocity_line,maxAccelation_line*accelerationRatio);
    velpref->SetProfile(0,pathC->PathLength());
    Trajectory* traject = new Trajectory_Segment(pathC, velpref);

    TrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp = new Trajectory_Composite();
    tmpTrajAttri.trajComp->Add(traject);
    double totalTime=tmpTrajAttri.trajComp->Duration();
    timeDurationResult.resize(2);
    timeDurationResult[0]=rad1/(rad1+rad2)*totalTime;
    timeDurationResult[1]=rad2/(rad1+rad2)*totalTime;

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=0;
    tmpTrajAttri.timeDuration=0;
    for(int i=0;i<timeDurationResult.size();i++)
    {
        tmpTrajAttri.timeDuration+=timeDurationResult[i];
    }
    addNewTrajComposite(tmpTrajAttri);
    positionFrameCommand_planed=positionIn.back();
    return 1;
}

int CoordinateTrajectory::getVehicleTargetPosition(VehiclePosition &valueOut,int &pointIndexOut)
{
    valueOut=targetPosition;
    pointIndexOut=targetPointIndex;
    return 1;
}

int CoordinateTrajectory::setPathInformation(QVector<VehiclePosition> targetPositionListIn, int pointIndexIn,
                                             E_NAV_COMMAND currentPathTypeIn,
                                             double currentPathParameter1In, double currentPathParameter2In)
{
    pathInfoMutex.lock();
    targetPositionList=targetPositionListIn;
    if(targetPositionList.size()>0)
    {
        targetPosition=targetPositionListIn.back();
    }
    startPosition=vehiclePositionFeedback;
    targetPointIndex=pointIndexIn;
    currentPathType=currentPathTypeIn;
    currentPathParameter1=currentPathParameter1In;
    currentPathParameter2=currentPathParameter2In;
    pathInfoMutex.unlock();
    return 1;
}



int CoordinateTrajectory::getCurrentPathInformation(VehiclePosition &startPositionOut, QVector<VehiclePosition> &targetPositionOut,
                                                    int &pointIndexOut,
               E_NAV_COMMAND &currentPathTypeOut, double &currentPathParameter1Out, double &currentPathParameter2Out)
{
    pathInfoMutex.lock();
    targetPositionOut=targetPositionList;
    startPositionOut=startPosition;
    pointIndexOut=targetPointIndex;
    currentPathTypeOut=currentPathType;
    currentPathParameter1Out=currentPathParameter1;
    currentPathParameter2Out=currentPathParameter2;
    pathInfoMutex.unlock();
    return 1;
}

int CoordinateTrajectory::synchronizePositionCommandWithFeedback()
{
    vehiclePositionCommand=vehiclePositionFeedback;
    vehiclePositionCommandOld=vehiclePositionCommand;

    positionFrameCommand=positionFrameFeedback;
    positionFrameCommand_planed=positionFrameCommand;
    positionFrameCommandOld=positionFrameCommand;

    positionCommand=positionFeedback;
    positionCommandOld=positionCommand;

    if(1)
    {
        qDebug()<<"CoordinateTrajectory::synchronizePositionCommandWithFeedback"
               <<positionFrameCommand.p[0]<<positionFrameCommand.p[1]<<positionFrameCommand.p[2];
    }

    return 1;

}

int CoordinateTrajectory::getAbsoluteVehiclePositionLeft(double &absoluteDistanceOut, double &absoluteAngleOut)
{
    absoluteDistanceOut=sqrt((targetPosition.x-vehiclePositionFeedback.x)*(targetPosition.x-vehiclePositionFeedback.x)
                             +(targetPosition.y-vehiclePositionFeedback.y)*(targetPosition.y-vehiclePositionFeedback.y));
    absoluteAngleOut=angleRotationJudgement_improved(targetPosition.rotate,vehiclePositionFeedback.rotate);
    return 1;
}

int CoordinateTrajectory::trapMove(PointPro positionIn,double speedPercent)
{
//    if(NULL!=trajComposite)
//    {
//        return -1;
//    }
//    trajComposite= new VelocityProfile_Trap(maxVelocity*speedPercent, maxAccelation*axisAccelerationRatio);
//    trajComposite->SetProfile(positionCommand, positionIn);
//    accumulatedTime=0;
    return 1;
}

int CoordinateTrajectory::trapMove(PointPro positionIn,double speedPercent,double acceleration)
{
//    if(NULL!=trajComposite)
//    {
//        return -1;
//    }
//    trajComposite= new VelocityProfile_Trap(maxVelocity*speedPercent, acceleration*axisAccelerationRatio);
//    trajComposite->SetProfile(positionCommand, positionIn);
//    accumulatedTime=0;
    return 1;
}


int CoordinateTrajectory::trapMoveByTime(PointPro positionIn,double timeDuration)
{
//    if(NULL!=trajComposite)
//    {
//        return -1;
//    }
//    trajComposite= new VelocityProfile_Trap(maxVelocity, maxAccelation*axisAccelerationRatio);
//    trajComposite->SetProfileDuration(positionCommand, positionIn,timeDuration);
//    accumulatedTime=0;
    return 1;
}

int CoordinateTrajectory::rectangularMove(PointPro positionIn,double speedPercent)
{
//    if(NULL!=trajComposite)
//    {
//        return -1;
//    }
//    trajComposite= new VelocityProfile_Rectangular(maxVelocity*speedPercent);
//    trajComposite->SetProfile(positionCommand, positionIn);
//    accumulatedTime=0;
    return 1;
}

int CoordinateTrajectory::splineZeroMove(PointPro positionIn,double speedPercent)
{
//    if(NULL!=trajComposite)
//    {
//        return -1;
//    }
//    trajComposite= new VelocityProfile_Spline();
//   double tmpTimeDuration=2.6*fabs(positionCommand-positionIn)/(maxVelocity*speedPercent);
//    ((VelocityProfile_Spline*)trajComposite)->SetProfileDuration(positionCommand, 0.0, 0.0, positionIn, 0.0, 0.0, tmpTimeDuration);
//    accumulatedTime=0;
    return 1;
}

int CoordinateTrajectory::splineZeroMoveByTime(PointPro positionIn,double timeDuration)
{
//    if(NULL!=trajComposite)
//    {
//        return -1;
//    }
//    trajComposite= new VelocityProfile_Spline();
//    ((VelocityProfile_Spline*)trajComposite)->SetProfileDuration(positionCommand, 0.0, 0.0, positionIn, 0.0, 0.0, timeDuration);
//    double tmpTimeDu=((VelocityProfile_Spline*)trajComposite)->Duration();
//    accumulatedTime=0;
    return 1;
}

int CoordinateTrajectory::splineContinueMove(PointPro positionIn,double velIn, double accIn)
{
//    if(NULL!=trajComposite)
//    {
//        return -1;
//    }
//    trajComposite= new VelocityProfile_Spline();
////    double tmpTimeDuration=4*fabs(positionCommand-positionIn)/fabs(velCommand+velIn);
//    double tmpTimeDuration=4*fabs(positionCommand-positionIn)/maxVelocity;
//    ((VelocityProfile_Spline*)trajComposite)->SetProfileDuration(positionCommand, velCommand, accCommand, positionIn, velIn, accIn, tmpTimeDuration);
//    accumulatedTime=0;
    return 1;
}



int CoordinateTrajectory::splineContinueMoveByTime(PointPro positionIn, double velIn, double accIn, double timeDuration)
{
//    if(NULL!=trajComposite)
//    {
//        return -1;
//    }
//    trajComposite= new VelocityProfile_Spline();
//    //五次样条曲线
//    ((VelocityProfile_Spline*)trajComposite)->SetProfileDuration(positionCommand, velCommand, accCommand
//                                                              , positionIn, velIn, accIn, timeDuration);
//   //３次样条曲线
//    //((VelocityProfile_Spline*)trajComposite)->SetProfileDuration(positionCommand, velCommand
//    //                                                          , positionIn, velIn, timeDuration);

//    accumulatedTime=0;
    return 1;
}

int CoordinateTrajectory::splineContinueMoveMultiPoint(QVector<VelCoordinatePoint> velPoint)
{
//    int pointCount=velPoint.size();
//    if(pointCount<1 || pointCount>VEL_PROFILE_ARRY_MAX)
//    {
//        return -1;
//    }
//    for(int i=0;i<pointCount;i++)
//    {
//        trajComposite= new VelocityProfile_Spline();
//        if(0==i)
//        {
//             ((VelocityProfile_Spline*)trajComposite)->SetProfileDuration(positionCommand, velCommand, accCommand
//                            , velPoint[i].positionIn, velPoint[i].velIn, velPoint[i].accIn, velPoint[i].timeDuration);
////            qDebug()<<" vel asix id="<<axisIdByRobot<<"point id="<<i+1 <<"data"<<velCommand << velPoint[i].velIn;
//        }
//        else if(i>0)
//        {
//            ((VelocityProfile_Spline*)trajComposite)->SetProfileDuration(velPoint[i-1].positionIn, velPoint[i-1].velIn, velPoint[i-1].accIn
//                           , velPoint[i].positionIn, velPoint[i].velIn, velPoint[i].accIn, velPoint[i].timeDuration);
////            qDebug()<<" vel asix id="<<axisIdByRobot<<"point id="<<i+1 <<"data"<< velPoint[i-1].velIn <<  velPoint[i].velIn;
//        }
//        trajCompositeArry[pointCount-1-i]=trajComposite;
//        trajComposite=NULL;// 不为空会导致无法添加新velProfile
//        currentTrajCompositeArryIndex=i;

//    }

//    trajComposite=trajCompositeArry[pointCount-1];//初始化，准备给调用生点,最后索引开始


    return 1;
}

int CoordinateTrajectory::getTimeDurationTrapMove(PointPro positionIn,double speedPercent, double& timeDuration)
{
//    if(speedPercent<0.001)
//    {
//        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5011,robotId, axisIdByRobot);
//        return -1;
//    }
//    VelocityProfile* tmpVelProfile= new VelocityProfile_Trap(maxVelocity*speedPercent, maxAccelation*axisAccelerationRatio);
//    tmpVelProfile->SetProfile(positionCommand, positionIn);
//    timeDuration=tmpVelProfile->Duration();
//    tmpVelProfile->~VelocityProfile();
//    accumulatedTime=0;
    return 1;
}

int CoordinateTrajectory::getTimeDurationSplineZeroMove(PointPro positionIn,double speedPercent, double& timeDuration)
{

//    if(speedPercent<0.001)
//    {
//        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5011,robotId, axisIdByRobot);
//        return -1;
//    }

//    VelocityProfile* tmpVelProfile= new VelocityProfile_Spline();
//    double tmpTimeDuration=2.6*fabs(positionCommand-positionIn)/(maxVelocity*speedPercent);
//    ((VelocityProfile_Spline*)tmpVelProfile)->SetProfileDuration(positionCommand, 0.0, 0.0, positionIn, 0.0, 0.0, tmpTimeDuration);
//    timeDuration=tmpVelProfile->Duration();
//    tmpVelProfile->~VelocityProfile();
//    accumulatedTime=0;

//    //for debug
//    timeDuration=tmpTimeDuration;
//    qDebug()<<"CoordinateTrajectory::getTimeDurationSplineZeroMove";
//    qDebug()<<"positionCommand="<<positionCommand<<"positionIn="<<positionIn<<"maxVelocity="<<maxVelocity
//           <<"speedPercent="<<speedPercent<<"tmpTimeDuration="<<tmpTimeDuration<<"timeDuration="<<timeDuration;

    return 1;

}

int CoordinateTrajectory::getTimeDurationSplineContinueMove(PointPro positionIn,double velIn, double& timeDuration)
{
//    VelocityProfile* tmpVelProfile= new VelocityProfile_Spline();
//    //TODO 时间怎么估算最合理？当开始和结束时间为０时，有问题。
////    double tmpTimeDuration=4*fabs(positionCommand-positionIn)/fabs(velCommand+velIn);
//    if(velIn<0.001)
//    {
//        addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5011,robotId, axisIdByRobot);
//        return -1;
//    }
//    double tmpTimeDuration=4*fabs(positionCommand-positionIn)/(maxVelocity*velIn);
//    ((VelocityProfile_Spline*)tmpVelProfile)->SetProfileDuration(positionCommand, 0.0, 0.0, positionIn, 0.0, 0.0, tmpTimeDuration);
//    timeDuration=tmpVelProfile->Duration();
//    tmpVelProfile->~VelocityProfile();
//    accumulatedTime=0;
    return 1;
}
 int CoordinateTrajectory::waitPositionFollowOffset()
 {
//     do
//     {
//         calculateFollowOffset();
//     }
//     while(fabs(positionFollowOffset)>targetFollowError);

     return 1;

 }




 int CoordinateTrajectory::isPositionCommandCheckOk(PointPro positionIn)
 {
//     if((positionIn > maxPosition) && ( positionIn >positionCommandOld) )
//     {
//         if(IF_DEBUG)
//         {
//             qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxPosition";
//         }
//          isPositionCommandExceed=2;
//          addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5001,robotId, axisIdByRobot);
//         return -1;
//     }
//     else if( (positionIn < minPosition) && ( positionIn <positionCommandOld) )
//     {
//         if(IF_DEBUG)
//         {
//             qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed minPosition";
//         }
//         isPositionCommandExceed=1;
//         addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5002,robotId, axisIdByRobot);
//         basicStatus=E_AXIS_STATUS_ERROR;
//         return -1;
//     }
//     positionCommandOld=positionIn;
    return 1;
 }

 int CoordinateTrajectory::isVelCommandCheckOk(double velIn)
 {
     if( E_CHECK_MODE_T1 == limitCheckMode )
     {
         if( fabs(velIn) > maxVelocity_line*t1RatioLimit*1.1 )
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5007,robotId, axisIdByRobot);
             if(IF_DEBUG)
             {
                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxVelocity";
             }
              isVelCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
             return -1;
         }
     }

     if( E_CHECK_MODE_T2 == limitCheckMode )
     {
         if( fabs(velIn) > maxVelocity_line  )
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5008,robotId, axisIdByRobot,velIn,maxVelocity_line);
             if(IF_DEBUG)
             {
                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxVelocity";
             }
              isVelCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
             return -1;
         }
     }

     return 1;
 }

 int CoordinateTrajectory::isVelBiasCheckOk(Twist velCommandIn, Twist velFeedbackIn)
 {
     //注意：只用了velFeedbackIn.vel[0]
#ifdef D_USE_ROS
     double tmp_velCommand=sqrt(velCommandIn.vel[0]*velCommandIn.vel[0]
             +velCommandIn.vel[1]*velCommandIn.vel[1]);
     double tmp_velFeedback=fabs(velFeedbackIn.vel[0]);
//     qDebug()<<"tmp_velCommand"<<tmp_velCommand<<"tmp_velFeedback"<<tmp_velFeedback
//            <<"allowVelFeedbackBiasRatio"<<allowVelFeedbackBiasRatio<<"tmp_exceedCount"<<tmp_exceedCount;

     //增加加速度判断，速度变大，才进行保护。
     double acc=(tmp_velFeedback-tmp_velFeedback_old)/deltaTime;
     tmp_velFeedback_old=tmp_velFeedback;
     if( tmp_velFeedback > tmp_velCommand*allowVelFeedbackBiasRatio  && tmp_velFeedback>0.20
             && acc>0.05)//tmp_velFeedback>0.05 报错
     {
         tmp_exceedCount++;
     }
     else
     {
          tmp_exceedCount=0;
     }
     if(tmp_exceedCount>18)//8还是报错0.17＞0.001
     {
         addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6007,robotId,tmp_velFeedback,
                tmp_velCommand*allowVelFeedbackBiasRatio,acc);
         if(IF_DEBUG)
         {
             qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxVelocity";
         }
          isVelCommandExceed=1;
          basicStatus=E_AXIS_STATUS_ERROR;
         return -1;
     }
#endif

     return 1;
 }

 int CoordinateTrajectory::isAccCommandCheckOk(double accIn)
 {
//     if( fabs(accIn) > maxAccelation )
//     {
//         addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5009,robotId, axisIdByRobot);
//         if(IF_DEBUG)
//         {
//             qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxAccelation";
//         }
//          isAccCommandExceed=1;
//          basicStatus=E_AXIS_STATUS_ERROR;
//         return -1;
//     }

    return 1;
 }

  int CoordinateTrajectory::isTorqueFeedbackCheckOk()
  {
//      return isTorqueCommandCheckOk(torqueFeedback);
      return 1;
  }

 int CoordinateTrajectory::isTorqueCommandCheckOk(double torqueIn)
 {

//     if( E_CHECK_MODE_T1 == limitCheckMode )
//     {
//         if( fabs(torqueIn) > jogTorqueLimit )
//         {
//             addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5010,robotId, axisIdByRobot);
//             if(IF_DEBUG)
//             {
//                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed jogTorqueLimit";
//             }
//             isTorqueCommandExceed=1;
//             basicStatus=E_AXIS_STATUS_ERROR;
//             return -1;
//         }
//     }
//     if( E_CHECK_MODE_T2 == limitCheckMode )
//     {
//         if( fabs(torqueIn) > maxTorque )
//         {
//             addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5010,robotId, axisIdByRobot);
//             if(IF_DEBUG)
//             {
//                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxTorque";
//             }
//             isTorqueCommandExceed=1;
//             basicStatus=E_AXIS_STATUS_ERROR;
//             return -1;
//         }
//     }
    return 1;
 }

 int CoordinateTrajectory::isFollowOffsetCheckOk(PointPro followOffsetIn)
 {
//     if( fabs(followOffsetIn) > processFollowError  )
//     {
//         addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",5015,robotId, axisIdByRobot);
//         if(IF_DEBUG)
//         {
//             qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed processFollowError";
//         }

//         isProcessFollowErrorExceed=1;
//         basicStatus=E_AXIS_STATUS_ERROR;
//         return -1;
//     }

    return 1;
 }

 int CoordinateTrajectory::isTargetOffsetCheckOk(PointPro followOffsetIn)
 {
//     if( fabs(followOffsetIn) > targetFollowError  )
//     {
//         return -1;
//     }
    return 1;
 }

 int CoordinateTrajectory::isTargetOffsetCheckOk()
 {
//     return isTargetOffsetCheckOk(positionFollowOffset);
     return 1;
 }

int CoordinateTrajectory::isLimitCheckOk()
{
    if(E_CHECK_EMERGENCY_STOP==limitCheckMode)
    {
        return 1;
    }

    if(1==isVelBiasCheckOk(velCommand,velFeedback))
    {
        return 1;
    }
    else
    {
        return -1;
    }
    return 1;
}

 int CoordinateTrajectory::setCheckMode(int modeIn)
 {
    limitCheckMode=modeIn;
     return 1;
 }

 double CoordinateTrajectory::getAccumulatedTime(int trajIndexIn)
 {

     if(trajAttributeQueue.size()<=trajIndexIn || trajIndexIn<0)
     {
         return -1;

     }
     else
     {
        return trajAttributeQueue[trajIndexIn].accumulatedTime;
     }
     return -1;

 }

 double CoordinateTrajectory::accumulatedTimeMoveOneStep(int trajIndexIn)
 {

     if(trajAttributeQueue.size()<=trajIndexIn || trajIndexIn<0)
     {
         return -1;

     }
     else
     {
        trajAttributeQueue[trajIndexIn].accumulatedTime+=timeAxisScale*deltaTime;
        return trajAttributeQueue[trajIndexIn].accumulatedTime;
     }
     return -1;
 }

 int CoordinateTrajectory::setAccumulatedTime(double timeIn, int trajIndexIn)
 {

     if(trajAttributeQueue.size()<=trajIndexIn || trajIndexIn<0)
     {
         return -1;

     }
     else
     {
        trajAttributeQueue[trajIndexIn].accumulatedTime=timeIn;
     }
     return 1;
 }

 Frame CoordinateTrajectory::getPositionFrameCommand()
 {
     return positionFrameCommand;
 }

 VehiclePosition CoordinateTrajectory::getVehiclePositionCommand()
 {
     return vehiclePositionCommand;
 }

 VehiclePosition CoordinateTrajectory::getVehiclePositionFeedback()
 {
     return vehiclePositionFeedback;
 }

 VehiclePosition CoordinateTrajectory::transferFrameToVehiclePosition(Frame frameIn)
 {
     VehiclePosition tmpPos;
     tmpPos.x=frameIn.p[0];
     tmpPos.y=frameIn.p[1];
     double roll;
     double pitch;
     frameIn.M.GetRPY(roll,pitch,tmpPos.rotate);
     return tmpPos;
 }

 int CoordinateTrajectory::setPositionFrameFeedback(Frame valueIn)
 {
//     if(1)
//     {
//         qDebug()<<"CoordinateTrajectory::setPositionFrameFeedback"<<valueIn.p[0]<<valueIn.p[1]<<valueIn.p[2];
//     }
     positionFrameFeedback=valueIn;
     setPositionFrameFeedback(transferFrameToVehiclePosition(valueIn));
     return 1;
 }

 int CoordinateTrajectory::setVehiclePositionFeedback(VehiclePosition valueIn,QVector<double> steeringPositionIn)
 {
     Frame tmpFrame=vehiclePositionToFrame(valueIn);
     positionFrameFeedback=tmpFrame;
     vehiclePositionFeedback=valueIn;
     for(int i=0;i<steeringPositionIn.size();i++ )
     {
         steeringPositionFeedback[i]= steeringPositionIn[i];
     }
     return 1;
 }

 int CoordinateTrajectory::setPositionFrameFeedback(VehiclePosition valueIn)
 {
     vehiclePositionFeedback=valueIn;
     return 1;
 }

 int CoordinateTrajectory::isTrajComposeEmpty()
 {
     //NULL==trajComposite
     if(0==trajAttributeQueue.size())
     {
         return true;
     }
     return false;
 }

 int CoordinateTrajectory::deleteTrajComposite()
 {
//     if(NULL!=trajComposite)
//     {
//         delete trajComposite;
//         //trajComposite->~VelocityProfile();
//         trajComposite=NULL;
//     }
     int tmpSize=trajAttributeQueue.size();
     for(int i=0;i<tmpSize;i++)
     {
         delete trajAttributeQueue[i].trajComp;
     }
     trajAttributeQueue.clear();
     return 1;
 }

// int CoordinateTrajectory::deleteTrajComposite_front()
// {
//     if(trajAttributeQueue.size()>0)
//     {
//         if(trajAttributeQueue.front().cnt>0)
//         {
//             accumulatedTime=accumulatedTime_secondTraj;
//             delete trajAttributeQueue.front().trajComp;
//             trajAttributeQueue.dequeue();


//         }
//         return 1;
//     }
//     return 0;
// }

 Trajectory_Composite *CoordinateTrajectory::getTrajComposite(int trajIndex)
 {
     if(trajIndex<trajAttributeQueue.size() && trajIndex>=0)
     {
         return trajAttributeQueue[trajIndex].trajComp;
     }
     return NULL;
 }

 int CoordinateTrajectory::addNewTrajComposite(TrajeAttribute attrIn)
 {
     attrIn.accumulatedTime=0;
     trajAttributeQueue.enqueue(attrIn);
     return 1;
 }

 int CoordinateTrajectory::setTrajCompositeEndCutoffTime(QVector<double> timeIn)
 {
     if(timeIn.size()!=trajAttributeQueue.size())
     {
         qDebug()<<"error,,,,timeIn.size()!=trajAttributeQueue.size()";
         return -1;
     }
     for(int i=0;i<timeIn.size();i++)
     {
         trajAttributeQueue[i].endCutoffTime=timeIn[i];
     }
     return 1;
 }

 int CoordinateTrajectory::setPositionFrameCommand(Frame valueIn)
 {
     if(0)
     {
         qDebug()<<"CoordinateTrajectory::setPositionFrameCommand"<<valueIn.p[0]<<valueIn.p[1]<<valueIn.p[2];
     }

     positionFrameCommand=valueIn;
     positionFrameCommand_planed=positionFrameCommand;
     return 1;
 }

 int CoordinateTrajectory::setPositionFrameCommand_planed(Frame valueIn)
 {
     if(0)
     {
         qDebug()<<"CoordinateTrajectory::setPositionFrameCommand_planed"<<valueIn.p[0]<<valueIn.p[1]<<valueIn.p[2];
     }
     positionFrameCommand_planed=valueIn;
     return 1;
 }

 double CoordinateTrajectory::angleRotationJudgement_improved(double targetAngle, double initialAngle)
 {
     targetAngle=transformToPi(targetAngle);
     initialAngle=transformToPi(initialAngle);
     double angle1,angle2,angle3;
     angle1=targetAngle-initialAngle;
     angle2=angle1+2*M_PI;
     angle3=angle1-2*M_PI;
     double minNum;
     if(fabs(angle1)<=fabs(angle2))
     {
         minNum=angle1;
     }
     else
     {
         minNum=angle2;
     }

     if(fabs(angle3)<=fabs(minNum))
     {
         minNum=angle3;
     }

     return minNum;
 }

 int CoordinateTrajectory::multiLineSmoothLinePlan(VehiclePosition startPos, QVector<VehiclePosition> passPosList,
                                                   double speedRatioIn, double cnt, QVector<Trajectory *> &trajListOut)
 {
     if(cnt<=0.000001)
     {
         qDebug()<<"multiLineSmoothLinePlan error, cnt<=0.000001 ";
         return -1;
     }

     trajListOut.clear();

     //first path
     VelocityProfile* velpref;
     Path_RoundedComposite* path = new Path_RoundedComposite(cnt/1000.0,cnt/1000.0,new RotationalInterpolation_SingleAxis(),true);
     int pointCount=passPosList.size();
     try
     {
             //将但其当前点写入第一个点
             path->Add(vehiclePositionToFrame(startPos));
             for(int i=0;i<pointCount;i++)
             {
                path->Add(vehiclePositionToFrame(passPosList[i]));
             }
             path->Finish();
     }
     catch(Error_MotionPlanning_Not_Feasible reason)
     {
         int errorCode=reason.GetType();

         switch(errorCode)
         {

             case 3101:
             {
                 qDebug()<<"lineContinuousMove error, eq. radius <= 0 "<<errorCode;
                 addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6001,robotId);
                 break;
             }
             case 3102:
             {
                 qDebug()<<"lineContinuousMove error,the first segment in a rounding has zero length "<<errorCode;
                 addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6002,robotId);
                 break;
             }
             case 3103:
             {
                 qDebug()<<"lineContinuousMove error,the second segment in a rounding has zero length "<<errorCode;
                 addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6003,robotId);
                 break;
             }
             case 3104:
             {
                 qDebug()<<"lineContinuousMove error,the angle between the first and the second segment is close to M_PI "<<errorCode;
                 addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6004,robotId);
                 break;
             }
             case 3105:
             {
                 qDebug()<<"lineContinuousMove error,the distance needed for the rounding is larger then the first segment "<<errorCode;
                 addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6016,robotId,cnt);
                 break;
             }
             case 3106:
             {
                 qDebug()<<"lineContinuousMove error,the distance needed for the rounding is larger then the second segment "<<errorCode;
                 addMsg(ENUM_MSG_ERROR,"MotionServer","CoordinateTrajectory",6006,robotId);
                 break;
             }

         }
         delete path;
         return -1;

     }

     velpref = new VelocityProfile_Trap(speedRatioIn*maxVelocity_line,maxAccelation_line*accelerationRatio);
     velpref->SetProfile(0,path->PathLength());
     Trajectory* traject = new Trajectory_Segment(path, velpref);
     trajListOut.append(traject);

     return 1;

 }

#define D_SPLINE_TIME_RATIO 2
 int CoordinateTrajectory::singleSmoothLinePlan(VehiclePosition startPos, VehiclePosition endPos, double speedRatioIn,
                                          QVector<Trajectory *> &trajListOut)
 {
     VehiclePosition passPos1,passPos2;
     double lineLength=sqrt((startPos.x-endPos.x)*(startPos.x-endPos.x)
                              +(startPos.y-endPos.y)*(startPos.y-endPos.y));
     double lineLength_x=endPos.x-startPos.x;
     double lineLength_y=endPos.y-startPos.y;

//     //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到匀速段。
//     if(lineLength<1.1)
//     {
//         trajListOut.resize(1);
//         return splineLinePlan(true,startPos,endPos,speedRatioIn,&trajListOut[0]);
//     }
     //样条曲线段固定长度，后面可以改进，如果总轨迹很长，可以加长样条段。
     //估计需要加速距离
     double t_accUp=speedRatioIn*maxVelocity_line/(maxAccelation_line*accelerationRatio);
     double startLength=0.5*maxAccelation_line*accelerationRatio*t_accUp*t_accUp;
     //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到匀速段。
     if(startLength>=lineLength/2.0)
     {
         //达不到要求速度，则根据加速度计算实际速度。
         double newTime=sqrt(2*lineLength/(maxAccelation_line*accelerationRatio));//1.2米距离，加速度非常高
         newTime=D_SPLINE_TIME_RATIO*newTime;

         trajListOut.resize(1);
         //用2.6的系数可能把加速度拉的非常高
         //用3.8系数，VMARK定位偏差最大２ｃｍ  ???
         //
         return splineLinePlan_timeDuration(newTime,startPos,endPos,&trajListOut[0]);
     }
//     double startLength=0.5;
     passPos1.x=startPos.x+startLength*(lineLength_x/lineLength);
     passPos1.y=startPos.y+startLength*(lineLength_y/lineLength);
     passPos1.rotate=startPos.rotate;

     double secondLength=lineLength-startLength;
     passPos2.x=startPos.x+secondLength*(lineLength_x/lineLength);
     passPos2.y=startPos.y+secondLength*(lineLength_y/lineLength);
     passPos2.rotate=startPos.rotate;

     trajListOut.clear();

     //first path
     VelocityProfile* velpref;
     Path_RoundedComposite* path = new Path_RoundedComposite(0.0,0.01,new RotationalInterpolation_SingleAxis(),true);
     path->Add(vehiclePositionToFrame(startPos));
     path->Add(vehiclePositionToFrame(passPos1));
     path->Finish();
     velpref= new VelocityProfile_Spline();
     double tmpTimeDuration=2.6*path->PathLength()/(maxVelocity_line*speedRatioIn);
     ((VelocityProfile_Spline*)velpref)->SetProfileDuration(0, 0.0, 0.0, path->PathLength(),
                                                            speedRatioIn*maxVelocity_line, 0.0, tmpTimeDuration);
     Trajectory* traject = new Trajectory_Segment(path, velpref);
     trajListOut.append(traject);

     //second path
     path = new Path_RoundedComposite(0.0,0.01,new RotationalInterpolation_SingleAxis(),true);
     path->Add(vehiclePositionToFrame(passPos1));
     path->Add(vehiclePositionToFrame(passPos2));
     path->Finish();
     //Error_MotionPlanning_Incompatible,acc()不允许被使用
//     velpref = new VelocityProfile_Rectangular(speedRatioIn*maxVelocity_line);
     velpref= new VelocityProfile_Spline();
     tmpTimeDuration=path->PathLength()/(maxVelocity_line*speedRatioIn);
     ((VelocityProfile_Spline*)velpref)->SetProfileDuration(0, maxVelocity_line*speedRatioIn, 0.0, path->PathLength(),
                                                            speedRatioIn*maxVelocity_line, 0.0, tmpTimeDuration);
     velpref->SetProfile(0,path->PathLength());
     traject = new Trajectory_Segment(path, velpref);
     trajListOut.append(traject);

     //third path
     path = new Path_RoundedComposite(0.0,0.01,new RotationalInterpolation_SingleAxis(),true);
     path->Add(vehiclePositionToFrame(passPos2));
     path->Add(vehiclePositionToFrame(endPos));
     path->Finish();
     velpref= new VelocityProfile_Spline();
     tmpTimeDuration=2.6*path->PathLength()/(maxVelocity_line*speedRatioIn);
     ((VelocityProfile_Spline*)velpref)->SetProfileDuration(0, speedRatioIn*maxVelocity_line, 0.0, path->PathLength(),
                                                            0, 0.0, tmpTimeDuration);
     traject = new Trajectory_Segment(path, velpref);
     trajListOut.append(traject);

     return 1;
 }

 int CoordinateTrajectory::improvedSplineLinePlan(E_LINE_VELOCITY_STYLE velStyle,VehiclePosition startPos, VehiclePosition endPos,
                                                  double speedRatioIn, Trajectory **trajOut)
 {
     VelocityProfile* velpref;
     Frame firstFrame=vehiclePositionToFrame(startPos);
     Frame secondFrame = vehiclePositionToFrame(endPos);
     Path_RoundedComposite* path = new Path_RoundedComposite(0.0,0.01,new RotationalInterpolation_SingleAxis(),true);
     path->Add(firstFrame);
     path->Add(secondFrame);
     path->Finish();

     // Trajectory defines a motion of the robot along a path.

     velpref = new VelocityProfile_ImprovedSpline(speedRatioIn*maxVelocity_line,maxAccelation_line*accelerationRatio);
     double tmpVelNew;
     switch(velStyle)
     {
     case E_LINE_VELOCITY_STYLE_ZERO_ZERO:
     {
         velpref->SetProfile(0,  path->PathLength());
         lastCommand_velocity_line=0;
         break;
     }
     case E_LINE_VELOCITY_STYLE_CHANGE_CONSTANT:
     {
         tmpVelNew=maxVelocity_line*speedRatioIn;
         ((VelocityProfile_ImprovedSpline*)velpref)->SetProfile_transsition(0,lastCommand_velocity_line,
                                                        path->PathLength(),tmpVelNew,true);
         lastCommand_velocity_line=tmpVelNew;
         break;
     }
     case E_LINE_VELOCITY_STYLE_CONSTANT_CHANGE:
     {
         tmpVelNew=0;
         ((VelocityProfile_ImprovedSpline*)velpref)->SetProfile_transsition(0,lastCommand_velocity_line,
                                                        path->PathLength(),tmpVelNew,false);
         lastCommand_velocity_line=0;
         break;
     }
     default:
     {
         qDebug()<<"error, unknow velocity plan1, lastCommand_velocity_line"<<lastCommand_velocity_line<<"velStyle"<<velStyle;
         delete path;
         delete velpref;
         return  -1;
     }
     }


     *trajOut = new Trajectory_Segment(path, velpref);

     return 1;
 }

 int CoordinateTrajectory::splineLinePlan(bool isVelocitySpline, double timeDurationEstimateRatio,VehiclePosition startPos, VehiclePosition endPos, double speedRatioIn,
                                          Trajectory ** trajOut)
 {
     VelocityProfile* velpref;
     Frame firstFrame=vehiclePositionToFrame(startPos);
     Frame secondFrame = vehiclePositionToFrame(endPos);
     Path_RoundedComposite* path = new Path_RoundedComposite(0.0,0.01,new RotationalInterpolation_SingleAxis(),true);
     path->Add(firstFrame);
     path->Add(secondFrame);
     path->Finish();

     // Trajectory defines a motion of the robot along a path.
     // This defines a trapezoidal velocity profile.
     if(true==isVelocitySpline)
     {
         //如果是纯粹旋转，则这个时间计算的不对。
         velpref= new VelocityProfile_Spline();
         //如果距离比较短，最大速度要求高，用2.6的系数可能把加速度拉的非常高
         double tmpTimeDuration=timeDurationEstimateRatio*path->PathLength()/(maxVelocity_line*speedRatioIn);
         if(tmpTimeDuration<D_MICRO_ANGLE_TIME)
         {
             tmpTimeDuration=D_MICRO_ANGLE_TIME;//提高末端定位微小的旋转纠偏。
         }
         ((VelocityProfile_Spline*)velpref)->SetProfileDuration(0, 0.0, 0.0, path->PathLength(), 0.0, 0.0, tmpTimeDuration);
     }
     else
     {
         velpref = new VelocityProfile_Trap(speedRatioIn*maxVelocity_line,maxAccelation_line*accelerationRatio);
         velpref->SetProfile(0,path->PathLength());
         if(velpref->Duration()<D_MICRO_ANGLE_TIME)
         {
             velpref->SetProfileDuration(0,path->PathLength(),D_MICRO_ANGLE_TIME);
         }

     }

     *trajOut = new Trajectory_Segment(path, velpref);

     return 1;
 }

 int CoordinateTrajectory::splineLinePlan_timeDuration(double timeDurationEstimateRatio, VehiclePosition startPos, VehiclePosition endPos, Trajectory **trajOut)
 {
     VelocityProfile* velpref;
     Frame firstFrame=vehiclePositionToFrame(startPos);
     Frame secondFrame = vehiclePositionToFrame(endPos);
     Path_RoundedComposite* path = new Path_RoundedComposite(0.0,0.01,new RotationalInterpolation_SingleAxis(),true);
     path->Add(firstFrame);
     path->Add(secondFrame);
     path->Finish();

     // Trajectory defines a motion of the robot along a path.
     // This defines a trapezoidal velocity profile.
     velpref= new VelocityProfile_Spline();
     ((VelocityProfile_Spline*)velpref)->SetProfileDuration(0, 0.0, 0.0, path->PathLength(), 0.0, 0.0, timeDurationEstimateRatio);


     *trajOut = new Trajectory_Segment(path, velpref);

     return 1;
 }



 int CoordinateTrajectory::splineRotatePlan(bool isVelocitySpline, VehiclePosition startPos,
                                            VehiclePosition endPos, double speedRatioIn, Trajectory **trajOut)
 {
     //x y z 坐标需要一样，要不然影响旋转速度的计算。
     VelocityProfile* velpref;
     Frame firstFrame=vehiclePositionToFrame(startPos);
     Frame secondFrame = vehiclePositionToFrame(endPos);
     Path_RoundedComposite* path = new Path_RoundedComposite(0.0,rotate_eqradius,new RotationalInterpolation_SingleAxis(),true);
     path->Add(firstFrame);
     path->Add(secondFrame);
     path->Finish();

     //让旋转在0.5达到最高速度。
     double tmp_rotateSpeed;
     if(speedRatioIn<0.5)
     {
         tmp_rotateSpeed=speedRatioIn*2*maxVelocity_rotate;

     }
     else
     {
         tmp_rotateSpeed=maxVelocity_rotate;
     }

     if(true==isVelocitySpline)
     {
         //如果是纯粹旋转，则这个时间计算的不对。
         velpref= new VelocityProfile_ImprovedSpline(tmp_rotateSpeed,maxAccelation_rotate*accelerationRatio);
         velpref->SetProfile(0, path->PathLength());
         if(velpref->Duration()<D_MICRO_ANGLE_TIME)
         {
             qDebug()<<"velpref->Duration()<D_MICRO_ANGLE_TIME";
             //为什么设定的是１ｓ,结果需要3.6S。x y z 坐标需要一样，要不然影响旋转速度的计算。
             velpref->SetProfileDuration(0, path->PathLength(),D_MICRO_ANGLE_TIME);//提高末端定位微小的旋转纠偏。
         }

     }
     else
     {

         velpref = new VelocityProfile_Trap(tmp_rotateSpeed,maxAccelation_rotate*accelerationRatio);
         velpref->SetProfile(0,path->PathLength());
         if(velpref->Duration()<D_MICRO_ANGLE_TIME)
         {
             velpref->SetProfileDuration(0,path->PathLength(),D_MICRO_ANGLE_TIME);
         }


     }

     *trajOut = new Trajectory_Segment(path, velpref);

     lastCommand_velocity_line=0;

     return 1;
 }

#define D_NO_STEERING_ANGLE 2
#define D_STEERING_EQRADIUS 300
 int CoordinateTrajectory::steeringRotatePlan(double startAngle, bool  isPrepareRotation,
                                              Trajectory **trajOut,double &targetAngleOut)
 {
     //距离判断
     if(isPrepareRotation)
     {
         if(fabs(startAngle-90)<D_NO_STEERING_ANGLE ||fabs(startAngle+90)<D_NO_STEERING_ANGLE)
         {
             qDebug()<<"旋转运动，转向偏差小，不用过渡 startAngle"<<startAngle;
             *trajOut=NULL;
             return 0;
         }
     }
     else
     {
         if(fabs(startAngle)<D_NO_STEERING_ANGLE)
         {
             qDebug()<<"直线运动，转向偏差小，不用过渡 startAngle"<<startAngle;
             *trajOut=NULL;
             return 0;
         }
     }

     //目标角度
     double tmpTargetAngle;
     if(isPrepareRotation)
     {
         if(fabs(startAngle-90)<fabs(startAngle+90))
         {
             tmpTargetAngle=90;
         }
         else
         {
             tmpTargetAngle=-90;
         }
     }
     else
     {
         tmpTargetAngle=0;
     }
     targetAngleOut=tmpTargetAngle;

     //规划
     VelocityProfile* velpref;
     VehiclePosition startPos,endPos;
     startPos.x=0;
     startPos.y=0;
     startPos.rotate=M_PI*startAngle/180.0;
     endPos.x=0;
     endPos.y=0;
     endPos.rotate=M_PI*tmpTargetAngle/180.0;
     Frame firstFrame=vehiclePositionToFrame(startPos);
     Frame secondFrame = vehiclePositionToFrame(endPos);
     Path_RoundedComposite* path = new Path_RoundedComposite(0.0,D_STEERING_EQRADIUS,new RotationalInterpolation_SingleAxis(),true);
     path->Add(firstFrame);
     path->Add(secondFrame);
     path->Finish();

     bool isVelocitySpline=false;
     if(true==isVelocitySpline)
     {
         //如果是纯粹旋转，则这个时间计算的不对。
         velpref= new VelocityProfile_ImprovedSpline(steeringMaxSpeed,steeringMaxAcceration);
         velpref->SetProfile(0, path->PathLength());
     }
     else
     {
         velpref = new VelocityProfile_Trap(steeringMaxSpeed,steeringMaxAcceration);
         velpref->SetProfile(0,path->PathLength());


     }

     *trajOut = new Trajectory_Segment(path, velpref);


     return 1;
 }

 int CoordinateTrajectory::waitTrajectoryPlan(VehiclePosition stationaryPose,double waitTimeIn,
                                              Trajectory **trajOut)
 {

      Frame firstFrame=vehiclePositionToFrame(stationaryPose);

     *trajOut = new Trajectory_Stationary(waitTimeIn, firstFrame);

     return 1;
 }

 double CoordinateTrajectory::transformToPi(double angleIn)
 {
     //    int turns=angleIn/M_PI/2;//可能bug 参考getTurnsFromRad
         int turns;

         if(0<angleIn)
         {
             turns=(angleIn+M_PI) /(2*M_PI);
         }
         else
         {
             turns=(angleIn-M_PI) /(2*M_PI);
         }

         //    angleIn=angleIn-turns*M_PI;//错误
             angleIn=angleIn-turns*M_PI*2;

         if(angleIn>=-M_PI && angleIn<=M_PI)
         {
             return angleIn;
         }
         else if(angleIn<-M_PI)
         {
             return angleIn+2*M_PI;
         }
         else if(angleIn>M_PI)
         {
             return angleIn-2*M_PI;
         }

         return angleIn;
 }

 int CoordinateTrajectory::addMsg(int messageLevel, string componentName, string messageType, int messageCode,
                                  int robotIdIn,
                                  double  parameter1, double parameter2, double parameter3, int parameter4)
 {
     Message tmpMsg;
     tmpMsg.MessageLevel = messageLevel;
     tmpMsg.componentClassName = componentName;
     tmpMsg.messageCode = messageCode;
     tmpMsg.messageType = messageType;
     tmpMsg.robotId = robotIdIn;

     string infomationStr;

     switch(messageCode)
     {
     case 6017:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("轨迹时间(%1)小于Mix过渡时间(%2)！").arg(parameter1).arg(parameter2).toStdString();
        infomationStr = str1;
        break;
     }
     case 6016:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("agv迪卡尔平滑规划失败，平滑半径(%1mm)大于第一段线段长度，不合理，请修改！检查初始位置是否合理！").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }
     case 6015:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("样条曲线的姿态过渡不合理,请修改路径点，允许姿态反方向偏差(%1度)！").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }
     case 6014:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("样条曲线的平均角速度超过允许值(%1度),请修改路径点或降低本段的速度！").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }
     case 6013:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("样条曲线的起始点姿态和起点到终点的矢量角度偏差大于允许值(%1度),请修改路径点！").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }
     case 6012:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("样条曲线的起始点和终止点距离小于允许值(%1度),请修改路径点！").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }
     case 6011:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("平滑样条曲线的末尾段加速度超过限制，请降低倒数第二段的速度，或者延长末尾段的距离，使可以平缓减速到０！").toStdString();
        infomationStr = str1;
        break;
     }
     case 6010:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("样条曲线的起始点和终止点角度偏差超过允许值(%1度),请修改路径点！").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }
     case 6009:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("坐标轴运动时间无穷小错误！").toStdString();
        infomationStr = str1;
        break;
     }
     case 6008:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("迪卡尔平滑规划失败，路径点个数少于２个！").toStdString();
        infomationStr = str1;
        break;
     }
     case 6007:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("轨迹反馈速度(%1)大于命令速度(%2),加速度(%3)").arg(parameter1).arg(parameter2).arg(parameter3)
                .toStdString();
        infomationStr = str1;
        break;
     }
     case 6006:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("迪卡尔平滑规划失败，平滑半径大于第二段线段长度，不合理，请修改！").toStdString();
        infomationStr = str1;
        break;
     }
     case 6005:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("机械臂迪卡尔平滑规划失败，平滑半径(%1mm)大于第一段线段长度，不合理，请修改！检查初始位置是否合理！").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }
     case 6004:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("迪卡尔平滑规划失败，第一二段直线之间夹角接近１８０度，不允许，请修改！").toStdString();
        infomationStr = str1;
        break;
     }
     case 6003:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("迪卡尔平滑规划失败，第二段直线距离太短，请加长！").toStdString();
        infomationStr = str1;
        break;
     }
     case 6002:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("迪卡尔平滑规划失败，第一段直线距离太短，请加长！").toStdString();
        infomationStr = str1;
        break;
     }
     case 6001:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("迪卡尔平滑规划失败,平滑半径参数小于０，请修改！").toStdString();
        infomationStr = str1;
        break;
     }

     case 6000:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("圆弧规划失败，点不符合圆弧要求，请修改点！").toStdString();
        infomationStr = str1;
        break;
     }

     case 5015:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")坐标第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴过程跟随误差超出限制").toStdString();
        infomationStr = str1;
        break;
     }
     case 5011:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴速度小于0.00１％，不合理！").toStdString();
            infomationStr = str1;
            break;
         }
         case 5010:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴力矩超出限制").toStdString();
            infomationStr = str1;
            break;
         }
         case 5009:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴加速度超出限制").toStdString();
            infomationStr = str1;
            break;
         }
         case 5008:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴命令速度(%1)超出T2限制(%2)").arg(parameter2).arg(parameter3).toStdString();
            infomationStr = str1;
            break;
         }
         case 5007:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴速度超出T1限制").toStdString();
            infomationStr = str1;
            break;
         }
       case 5002:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴位置超出负限位").toStdString();
            infomationStr = str1;
            break;
         }
         case 5001:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴位置超出正限位").toStdString();
            infomationStr = str1;
            break;
         }





         default:
         {
             qDebug()<<"Coordinate::addMsg====not match error code";
             break;
         }
     }

     tmpMsg.MessageInformation = infomationStr;

     #ifndef MOTION_SERVER_UNIT_TEST
     motionMessage->addMsg(tmpMsg);
     #endif
 }




int CoordinateTrajectory::processLineInstruction(MoveParameter moveParameter)
{
//    printInformation(moveParameter,"processLineInstruction");

////     if(moveParameter.targetPoint.positionValue.size()<jointsNum)
////     {
////         qDebug()<<"TrajectoryServer:: processLineInstruction moveParameter error!!!";
////         return -1;
////     }

//        // Path_RoundedComposite defines the geometric path along
//        // which the robot will move.
//        Path_RoundedComposite* path = new Path_RoundedComposite(0.2,0.01,new RotationalInterpolation_SingleAxis(),true);
//        //将但其当前点写入第一个点
//        path->Add(Frame(pointProToFrame(commandPoint));
//        path->Add(Frame(Rotation::RPY(moveParameter.targetPoint.positionValue[3]
//                                      ,moveParameter.targetPoint.positionValue[4]
//                                      ,moveParameter.targetPoint.positionValue[5])
//                        , Vector(moveParameter.targetPoint.positionValue[0]
//                                 ,moveParameter.targetPoint.positionValue[1]
//                                 ,moveParameter.targetPoint.positionValue[2])));
//        path->Finish();

//        // Trajectory defines a motion of the robot along a path.
//        // This defines a trapezoidal velocity profile.
//        VelocityProfile* velpref = new VelocityProfile_Trap(0.5,0.1);
//        velpref->SetProfile(0,path->PathLength());
//        Trajectory* traject = new Trajectory_Segment(path, velpref);


//        if(NULL!=ctraject)
//        {
//            return -1;
//        }
//        else
//        {
//           TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C7]);
//           ctraject = new Trajectory_Composite();
//           ctraject->Add(traject);
//  //       ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))));
//           trajTimeDuration=ctraject->Duration();
//           addTrajTime(moveParameter.lineNumber,trajTimeDuration);
//           TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C7]);
//        }


        return 1;

//         // use the trajectory
//         double dt=0.1;
//         std::ofstream of("./trajectory.dat");
//         for (double t=0.0; t <= traject->Duration(); t+= dt) {
//             Frame current_pose;
//             current_pose = traject->Pos(t);
//             for (int i=0;i<4;++i)
//                 for (int j=0;j<4;++j)
//                     of << current_pose(i,j) << "\t";
//             of << "\n";
//             // also velocities and accelerations are available !
//             //traject->Vel(t);
//             //traject->Acc(t);
//         }
//         of.close();

//         // you can get some meta-info on the path:
//         for (int segmentnr=0;  segmentnr < path->GetNrOfSegments(); segmentnr++) {
//             double starts,ends;
//             Path::IdentifierType pathtype;
//             if (segmentnr==0) {
//                 starts = 0.0;
//             } else {
//                 starts = path->GetLengthToEndOfSegment(segmentnr-1);
//             }
//             ends = path->GetLengthToEndOfSegment(segmentnr);
//             pathtype = path->GetSegment(segmentnr)->getIdentifier();
//             std::cout << "segment " << segmentnr << " runs from s="<<starts << " to s=" <<ends;
//             switch(pathtype) {
//                 case Path::ID_CIRCLE:
//                     std::cout << " circle";
//                     break;
//                 case Path::ID_LINE:
//                     std::cout << " line ";
//                     break;
//                 default:
//                     std::cout << " unknown ";
//                     break;
//             }
//             std::cout << std::endl;
//         }
//         std::cout << " trajectory written to the ./trajectory.dat file " << std::endl;

        //delete ctraject;


}

int CoordinateTrajectory::processCircleInstruction(MoveParameter moveParameter)
{
//   addTrajTime(moveParameter.lineNumber,trajTimeDuration);
   return 1;
}



int CoordinateTrajectory::processCartesianContinousInstruction(QVector<MoveParameter> moveParameterArry, int isLastPointMove)
{
//    printInformation( moveParameterArry, "pprocessJointContinousInstruction");

//    int pointCount=moveParameterArry.size();
//    if(0==pointCount)
//    {
//        qDebug()<<"error: TrajectoryServer:: processCartesianContinousInstruction 0==pointCount!";
//        return -1;
//    }
//    for(int i=0;i<pointCount;i++)
//    {
//        //检查数据轴数合法性
//        if(moveParameterArry[i].targetPoint.positionValue.size()<jointsNum)
//        {
//            qDebug()<<"TrajectoryServer:: processJointContinousInstruction moveParameter error!!!";
//            return -1;
//        }
//    }


//   Path_RoundedComposite* path = new Path_RoundedComposite(0.2,0.01,new RotationalInterpolation_SingleAxis(),true);
//   //TODO 将但其当前点写入第一个点
//   path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0)));
//   for(int k =0;k<pointCount;k++)
//   {
//       path->Add(Frame(Rotation::RPY(moveParameterArry[k].targetPoint.positionValue[3]
//                                     ,moveParameterArry[k].targetPoint.positionValue[4]
//                                     ,moveParameterArry[k].targetPoint.positionValue[5])
//                       , Vector(moveParameterArry[k].targetPoint.positionValue[0]
//                                ,moveParameterArry[k].targetPoint.positionValue[1]
//                                ,moveParameterArry[k].targetPoint.positionValue[2])));

//   }

//   path->Finish();
//   VelocityProfile* velpref = new VelocityProfile_Trap(0.5,0.1);//max vel, max acc
//   velpref->SetProfile(0,path->PathLength());
//   Trajectory* traject = new Trajectory_Segment(path, velpref);
//   // TO do 可以通过添加ctraject->Add(traject)，添加多个段，从而控制不同段的不同速度

//   if(NULL!=ctraject)
//   {
//       return -1;
//   }
//   else
//   {

//      TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C9]);
//      ctraject = new Trajectory_Composite();
//      ctraject->Add(traject);
//      trajTimeDuration=ctraject->Duration();

//      addTrajTime(moveParameterArry[0].lineNumber,trajTimeDuration);//todo 没有分path
//      TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C9]);


//   }

   return 1;
}
